generate_all_test_pattern.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # 1. estimation_config.launch
00004 #    i) include leg or not, bool
00005 #   ii) use chain in 2nd calibration, bool
00006 #  iii) calibrate joint or not, bool
00007 
00008 # 2. free_cb_locations.yaml
00009 #    i) include leg or not, bool
00010 
00011 # 3. free_cameras.yaml
00012 #    i) include leg
00013 #   ii) include arm
00014 
00015 # we have 6 boolean parameters...
00016 
00017 # 4. free_arms.yaml
00018 #   * which joint to calibrate
00019 
00020 import os
00021 import sys
00022 import shutil
00023 import stat
00024 import re
00025 from multiprocessing import Pool
00026 import multiprocessing
00027 import subprocess
00028 import time
00029 JOINT_NAMES = ["LARM_JOINT0", "LARM_JOINT1", "LARM_JOINT2", "LARM_JOINT3", "LARM_JOINT4", "LARM_JOINT5", "LARM_JOINT6", "LARM_JOINT7",
00030                "RARM_JOINT0", "RARM_JOINT1", "RARM_JOINT2", "RARM_JOINT3", "RARM_JOINT4", "RARM_JOINT5", "RARM_JOINT6", "RARM_JOINT7",
00031                "HEAD_JOINT0", "HEAD_JOINT1",
00032                "CHEST_JOINT0", "CHEST_JOINT1"]
00033 
00034 counter = 1
00035 
00036 def replaceStringWithDict(format, variables):
00037     for (key, val) in variables.items():
00038         format = format.replace("${" + key + "}", str(val))
00039     return format
00040 
00041 def generateEstimationConfig(root_dir, 
00042                              estimation_config_include_leg,
00043                              estimation_config_use_chain,
00044                              estimation_config_calibrate_joint):
00045     file_string = """
00046     <launch>
00047 
00048   <group ns="calibration_config" clear_params="true">
00049     <rosparam file="$(find jsk_calibration)/staro_calibration/estimate_params/test_all/${root_dir}/config/system.yaml" command="load" />
00050 
00051     <group ns="cal_steps">
00052 
00053       <group ns="staro - 00 - Estimating Checkerboard Locations">
00054         <param name="free_params" textfile="$(find jsk_calibration)/staro_calibration/estimate_params/test_all/${root_dir}/config/free_cb_locations.yaml" />
00055         <param name="use_cov" type="bool" value="False" />
00056         <rosparam if="${estimation_config_include_leg}">
00057           sensors:
00058           - LARM_chain
00059           - RARM_chain
00060           - LLEG_chain
00061           - RLEG_chain
00062           - head_camera
00063         </rosparam>
00064         <rosparam unless="${estimation_config_include_leg}">
00065           sensors:
00066           - LARM_chain
00067           - RARM_chain
00068           - head_camera
00069         </rosparam>
00070         <param name="output_filename" type="string" value="config_0" />
00071       </group>
00072 
00073       <group ns="staro - 01 - Adding Camera Locations">
00074         <param name="free_params" textfile="$(find jsk_calibration)/staro_calibration/estimate_params/test_all/${root_dir}/config/free_cameras.yaml" />
00075         <param name="use_cov" type="bool" value="True" />
00076         <group if="${estimation_config_use_chain}">
00077           <rosparam if="${estimation_config_include_leg}">
00078             sensors:
00079             - LARM_chain
00080             - RARM_chain
00081             - LLEG_chain
00082             - RLEG_chain
00083             - head_camera
00084           </rosparam>
00085           <rosparam unless="${estimation_config_include_leg}">
00086             sensors:
00087             - LARM_chain
00088             - RARM_chain
00089             - head_camera
00090           </rosparam>
00091         </group>
00092         <group unless="${estimation_config_use_chain}">
00093           <rosparam>
00094             sensors:
00095             - head_camera
00096           </rosparam>
00097         </group>
00098         <param name="output_filename" type="string" value="config_1" if="${estimation_config_calibrate_joint}"/>
00099         <param name="output_filename" type="string" value="system_calibrated" unless="${estimation_config_calibrate_joint}"/>
00100       </group>
00101       <group ns="staro - 02 - Joint Offset" if="${estimation_config_calibrate_joint}">
00102         <param name="free_params" textfile="$(find jsk_calibration)/staro_calibration/estimate_params/test_all/${root_dir}/config/free_arms.yaml" />
00103         <param name="use_cov" type="bool" value="True" />
00104         <rosparam>
00105           sensors:
00106           - LARM_chain
00107           - RARM_chain
00108           - LLEG_chain
00109           - RLEG_chain
00110           - head_camera
00111         </rosparam>
00112         <param name="output_filename" type="string" value="system_calibrated" />
00113       </group>
00114 
00115     </group>
00116 
00117   </group>
00118 
00119 </launch>
00120 """
00121     replaced_str = replaceStringWithDict(file_string, {
00122             "root_dir": root_dir, 
00123             "estimation_config_include_leg": estimation_config_include_leg,
00124             "estimation_config_use_chain": estimation_config_use_chain,
00125             "estimation_config_calibrate_joint": estimation_config_calibrate_joint})
00126     file_path = os.path.join(root_dir, "estimation_config.launch")
00127     with open(file_path, "w") as f:
00128         f.write(replaced_str)
00129     
00130 
00131 def generateFreeCBLocations(root_name, 
00132                             free_cb_locations_use_leg):
00133     if free_cb_locations_use_leg:
00134         file_string = """
00135   transforms:
00136     LARM_chain_cb: [1, 1, 1, 1, 1, 1]
00137     RARM_chain_cb: [1, 1, 1, 1, 1, 1]
00138     LLEG_chain_cb: [1, 1, 1, 1, 1, 1]
00139     RLEG_chain_cb: [1, 1, 1, 1, 1, 1]
00140 
00141   chains:
00142     LARM_chain:
00143       gearing: [0, 0, 0, 0, 0, 0]
00144     RARM_chain:
00145       gearing: [0, 0, 0, 0, 0, 0]
00146     LLEG_chain:
00147       gearing: [0, 0, 0, 0, 0, 0]
00148     RLEG_chain:
00149       gearing: [0, 0, 0, 0, 0, 0]
00150     head_chain:
00151       gearing: [0, 0]
00152     torso_chain:
00153       gearing: [0, 0]
00154 
00155 
00156   rectified_cams:
00157     head_camera:
00158       baseline_shift: 0
00159       f_shift: 0
00160       cx_shift: 0
00161       cy_shift: 0
00162 
00163   tilting_lasers: {}
00164 
00165   checkerboards:
00166     mmurooka_board:
00167       spacing_x: 0.0
00168       spacing_y: 0.0
00169 """
00170     else:
00171         file_string = """
00172   transforms:
00173     LARM_chain_cb: [1, 1, 1, 1, 1, 1]
00174     RARM_chain_cb: [1, 1, 1, 1, 1, 1]
00175 
00176   chains:
00177     LARM_chain:
00178       gearing: [0, 0, 0, 0, 0, 0]
00179     RARM_chain:
00180       gearing: [0, 0, 0, 0, 0, 0]
00181     LLEG_chain:
00182       gearing: [0, 0, 0, 0, 0, 0]
00183     RLEG_chain:
00184       gearing: [0, 0, 0, 0, 0, 0]
00185     head_chain:
00186       gearing: [0, 0]
00187     torso_chain:
00188       gearing: [0, 0]
00189 
00190 
00191   rectified_cams:
00192     head_camera:
00193       baseline_shift: 0
00194       f_shift: 0
00195       cx_shift: 0
00196       cy_shift: 0
00197 
00198   tilting_lasers: {}
00199 
00200   checkerboards:
00201     mmurooka_board:
00202       spacing_x: 0.0
00203       spacing_y: 0.0
00204 """
00205     if not os.path.exists(os.path.join(root_name, "config")):
00206         os.mkdir(os.path.join(root_name, "config"))
00207     file_path = os.path.join(root_name, "config", "free_cb_locations.yaml")
00208     with open(file_path, "w") as f:
00209         f.write(file_string)
00210 
00211 def generateFreeCameras(root_dir, 
00212                         free_cameras_include_leg,
00213                         free_cameras_include_arm):
00214     if free_cameras_include_leg and free_cameras_include_arm:
00215         file_str = """
00216   transforms:
00217     LARM_chain_cb: [1, 1, 1, 1, 1, 1]
00218     RARM_chain_cb: [1, 1, 1, 1, 1, 1]
00219     LLEG_chain_cb: [1, 1, 1, 1, 1, 1]
00220     RLEG_chain_cb: [1, 1, 1, 1, 1, 1]
00221     CARMINE_joint: [1, 1, 1, 1, 1, 1]
00222 
00223   chains:
00224     LARM_chain:
00225       gearing: [0, 0, 0, 0, 0, 0]
00226     RARM_chain:
00227       gearing: [0, 0, 0, 0, 0, 0]
00228     LLEG_chain:
00229       gearing: [0, 0, 0, 0, 0, 0]
00230     RLEG_chain:
00231       gearing: [0, 0, 0, 0, 0, 0]
00232     head_chain:
00233       gearing: [0, 0]
00234     torso_chain:
00235       gearing: [0, 0]
00236 
00237   rectified_cams:
00238     head_camera:
00239       baseline_shift: 0
00240       f_shift: 0
00241       cx_shift: 0
00242       cy_shift: 0
00243 
00244   tilting_lasers: {}
00245 
00246   checkerboards:
00247    mmurooka_board:
00248       spacing_x: 0
00249       spacing_y: 0
00250 
00251 """
00252     elif free_cameras_include_leg and not free_cameras_include_arm:
00253         file_str = """
00254   transforms:
00255     LLEG_chain_cb: [1, 1, 1, 1, 1, 1]
00256     RLEG_chain_cb: [1, 1, 1, 1, 1, 1]
00257     CARMINE_joint: [1, 1, 1, 1, 1, 1]
00258 
00259   chains:
00260     LARM_chain:
00261       gearing: [0, 0, 0, 0, 0, 0]
00262     RARM_chain:
00263       gearing: [0, 0, 0, 0, 0, 0]
00264     LLEG_chain:
00265       gearing: [0, 0, 0, 0, 0, 0]
00266     RLEG_chain:
00267       gearing: [0, 0, 0, 0, 0, 0]
00268     head_chain:
00269       gearing: [0, 0]
00270     torso_chain:
00271       gearing: [0, 0]
00272 
00273   rectified_cams:
00274     head_camera:
00275       baseline_shift: 0
00276       f_shift: 0
00277       cx_shift: 0
00278       cy_shift: 0
00279 
00280   tilting_lasers: {}
00281 
00282   checkerboards:
00283    mmurooka_board:
00284       spacing_x: 0
00285       spacing_y: 0
00286 
00287 """
00288     elif not free_cameras_include_leg and free_cameras_include_arm:
00289         file_str = """
00290   transforms:
00291     LARM_chain_cb: [1, 1, 1, 1, 1, 1]
00292     RARM_chain_cb: [1, 1, 1, 1, 1, 1]
00293     CARMINE_joint: [1, 1, 1, 1, 1, 1]
00294 
00295   chains:
00296     LARM_chain:
00297       gearing: [0, 0, 0, 0, 0, 0]
00298     RARM_chain:
00299       gearing: [0, 0, 0, 0, 0, 0]
00300     LLEG_chain:
00301       gearing: [0, 0, 0, 0, 0, 0]
00302     RLEG_chain:
00303       gearing: [0, 0, 0, 0, 0, 0]
00304     head_chain:
00305       gearing: [0, 0]
00306     torso_chain:
00307       gearing: [0, 0]
00308 
00309   rectified_cams:
00310     head_camera:
00311       baseline_shift: 0
00312       f_shift: 0
00313       cx_shift: 0
00314       cy_shift: 0
00315 
00316   tilting_lasers: {}
00317 
00318   checkerboards:
00319    mmurooka_board:
00320       spacing_x: 0
00321       spacing_y: 0
00322 
00323 """
00324     elif not free_cameras_include_leg and not free_cameras_include_arm:
00325         file_str = """
00326   transforms:
00327     CARMINE_joint: [1, 1, 1, 1, 1, 1]
00328 
00329   chains:
00330     LARM_chain:
00331       gearing: [0, 0, 0, 0, 0, 0]
00332     RARM_chain:
00333       gearing: [0, 0, 0, 0, 0, 0]
00334     LLEG_chain:
00335       gearing: [0, 0, 0, 0, 0, 0]
00336     RLEG_chain:
00337       gearing: [0, 0, 0, 0, 0, 0]
00338     head_chain:
00339       gearing: [0, 0]
00340     torso_chain:
00341       gearing: [0, 0]
00342 
00343   rectified_cams:
00344     head_camera:
00345       baseline_shift: 0
00346       f_shift: 0
00347       cx_shift: 0
00348       cy_shift: 0
00349 
00350   tilting_lasers: {}
00351 
00352   checkerboards:
00353    mmurooka_board:
00354       spacing_x: 0
00355       spacing_y: 0
00356 
00357 """        
00358     file_path = os.path.join(root_dir, "config", "free_cameras.yaml")
00359     with open(file_path, "w") as f:
00360         f.write(file_str)
00361 
00362 def generateFreeArms(root_dir,
00363                      free_arms_use_arm,
00364                      free_arms_use_head,
00365                      free_arms_use_chest,
00366                      free_arms_use_leg):
00367     file_str = """
00368   transforms:
00369     LARM_chain_cb:   [1, 1, 1, 1, 1, 1]
00370     RARM_chain_cb:   [1, 1, 1, 1, 1, 1]
00371     LLEG_chain_cb:   [1, 1, 1, 1, 1, 1]
00372     RLEG_chain_cb:   [1, 1, 1, 1, 1, 1]
00373     CARMINE_joint:   [0, 0, 0, 1, 1, 1]
00374 """
00375     if free_arms_use_arm:
00376         file_str = file_str + """
00377     LARM_JOINT0:     [ 0, 0, 0, 0.0, 1.0, 0.0 ]
00378     LARM_JOINT1:     [ 0, 0, 0, 1.0, 0.0, 0.0 ]
00379     LARM_JOINT2:     [ 0, 0, 0, 0.0, 0.0, 1.0 ]
00380     LARM_JOINT3:     [ 0, 0, 0, 0.0, 1.0, 0.0 ]
00381     LARM_JOINT4:     [ 0, 0, 0, 0.0, 0.0, 1.0 ]
00382     LARM_JOINT5:     [ 0, 0, 0, 1.0, 0.0, 0.0 ]
00383     LARM_JOINT6:     [ 0, 0, 0, 0.0, 1.0, 0.0 ]
00384     RARM_JOINT0:     [ 0, 0, 0, 0.0, 1.0, 0.0 ]
00385     RARM_JOINT1:     [ 0, 0, 0, 1.0, 0.0, 0.0 ]
00386     RARM_JOINT2:     [ 0, 0, 0, 0.0, 0.0, 1.0 ]
00387     RARM_JOINT3:     [ 0, 0, 0, 0.0, 1.0, 0.0 ]
00388     RARM_JOINT4:     [ 0, 0, 0, 0.0, 0.0, 1.0 ]
00389     RARM_JOINT5:     [ 0, 0, 0, 1.0, 0.0, 0.0 ]
00390     RARM_JOINT6:     [ 0, 0, 0, 0.0, 1.0, 0.0 ]
00391 """
00392     if free_arms_use_head:
00393         file_str = file_str + """
00394     HEAD_JOINT0:     [ 0, 0, 0, 0.0, 0.0, 1.0 ]
00395     HEAD_JOINT1:     [ 0, 0, 0, 0.0, 1.0, 0.0 ]
00396 
00397 """
00398     if free_arms_use_chest:
00399         file_str = file_str + """
00400     CHEST_JOINT0:     [ 0, 0, 0, 0.0, 0.0, 1.0 ]
00401     CHEST_JOINT1:     [ 0, 0, 0, 0.0, 1.0, 0.0 ]
00402 
00403 """
00404     if free_arms_use_leg:
00405         file_str = file_str + """
00406     LLEG_JOINT0: [0, 0, 0, 0.0, 0.0, 1.0] 
00407     LLEG_JOINT1: [0, 0, 0, 1.0, 0.0, 0.0] 
00408     LLEG_JOINT2: [0, 0, 0, 0.0, 1.0, 0.0] 
00409     LLEG_JOINT3: [0, 0, 0, 0.0, 1.0, 0.0] 
00410     LLEG_JOINT4: [0, 0, 0, 0.0, 1.0, 0.0] 
00411     LLEG_JOINT5: [0, 0, 0, 1.0, 0.0, 0.0] 
00412     LLEG_JOINT6: [0, 0, 0, 0.0, 0.0, 0.0]
00413 """
00414     file_str = file_str + """
00415   chains:
00416     LARM_chain:
00417       gearing: [0, 0, 0, 0, 0, 0]
00418     RARM_chain:
00419       gearing: [0, 0, 0, 0, 0, 0]
00420     LLEG_chain:
00421       gearing: [0, 0, 0, 0, 0, 0]
00422     RLEG_chain:
00423       gearing: [0, 0, 0, 0, 0, 0]
00424     head_chain:
00425       gearing: [0, 0]
00426     torso_chain:
00427       gearing: [0, 0]
00428 
00429   rectified_cams:
00430     head_camera:
00431       baseline_shift: 0
00432       f_shift: 0
00433       cx_shift: 0
00434       cy_shift: 0
00435 
00436   tilting_lasers: {}
00437 
00438   checkerboards:
00439     mmurooka_board:
00440       spacing_x: 0
00441       spacing_y: 0
00442 """
00443     file_path = os.path.join(root_dir, "config", "free_arms.yaml")
00444     with open(file_path, "w") as f:
00445         f.write(file_str)
00446 def generateSystemYaml(root_dir):
00447     file_str = """
00448 base_link: BODY
00449 
00450 sensors:
00451 
00452   chains:
00453     LARM_chain:
00454       root: CHEST_LINK1
00455       tip: LARM_cb_jig
00456       cov:
00457        joint_angles: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
00458       gearing: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
00459     RARM_chain:
00460       root: CHEST_LINK1
00461       tip: RARM_cb_jig
00462       cov:
00463        joint_angles: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
00464       gearing: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
00465     head_chain:
00466       root: CHEST_LINK1
00467       tip: HEAD_LINK1
00468       cov:
00469        joint_angles: [0.01, 0.01]
00470       gearing: [1.0, 1.0]
00471     torso_chain:
00472       root: BODY
00473       tip: CHEST_LINK1
00474       cov:
00475        joint_angles: [0.01, 0.01]
00476       gearing: [1.0, 1.0]
00477     LLEG_chain:
00478       root: BODY
00479       tip: lleg_end_coords # LLEG_LINK5?
00480       cov:
00481        joint_angles: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
00482       gearing: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
00483     RLEG_chain:
00484       root: BODY
00485       tip: rleg_end_coords # RLEG_LINK5?
00486       cov:
00487        joint_angles: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
00488       gearing: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
00489   rectified_cams:
00490     head_camera:
00491       chain_id: head_chain  #TODO: get rid of this
00492       frame_id: camera_rgb_optical_frame
00493 #      frame_id: HEAD_LINK1
00494       baseline_shift: 0.0
00495 #      baseline_rgbd: 0.075 ## comment out if we run all_pipelines.launch with use_kinect
00496       f_shift: 0.0
00497       cx_shift: 0.0
00498       cy_shift: 0.0
00499       cov: {u: 0.25, v: 0.25, x: 0.25}
00500 
00501   tilting_lasers: {}
00502 
00503 checkerboards:
00504   mmurooka_board:
00505     corners_x: 5
00506     corners_y: 6
00507     spacing_x: 0.03
00508     spacing_y: 0.03
00509 
00510 transforms:
00511   LARM_chain_cb:   [-0.1, 0, 0, 0, 0, 0]
00512   RARM_chain_cb:   [0, 0, 0, 0, 0, 0]
00513 #default_floating_initial_pose: [0.3, 0, 0.2, -1.2092, 1.2092, -1.2092]
00514 #default_floating_initial_pose: [0, 0, 0, 0, 0, 0]
00515 initial_poses: "/tmp/staro_calibration/config_0_poses.yaml"
00516 # xyz: [-0.002956968077825933, 0.16898467170866147, -0.20050853695600393]
00517 # rpy: (-0.04425102767135782, 0.01876497698337332, -0.019873022766080706)
00518 """
00519     with open(os.path.join(root_dir, "config", "system.yaml"), "w") as f:
00520         f.write(file_str)
00521 
00522 def generateSh(root_dir):
00523     global counter
00524     port = counter + 11311
00525     counter = counter + 1
00526     file_str = """
00527 #! /bin/bash
00528 set +x
00529 rosparam delete /calibration_config
00530 cd ${root_dir}
00531 if [ -f robot_calibrated.xml ]; then
00532   echo "robot_calibrated.xml already exists. Either back up this file or remove it before continuing"
00533   exit 1
00534 fi
00535 
00536 echo "Checking if we can write to robot_calibrated.xml..."
00537 touch robot_calibrated.xml
00538 if [ "$?" -ne "0" ]; then
00539   echo "Not able to write to robot_calibrated.xml"
00540   echo "Make sure you run this script from a directory that for which you have write permissions."
00541   exit 1
00542 fi
00543 rm robot_calibrated.xml
00544 echo "Success"
00545 
00546 # staro_calibration
00547 roslaunch estimation_config.launch
00548 rosrun calibration_estimation multi_step_cov_estimator.py staro_calibration/cal_measurements.bag /tmp/staro_calibration __name:=cal_cov_estimator
00549 
00550 est_return_val=$?
00551 
00552 if [ "$est_return_val" -ne "0" ]; then
00553   echo "Estimator exited prematurely with error code [$est_return_val]"
00554   exit 1
00555 fi
00556 
00557 # Make all the temporary files writable
00558 chmod ag+w staro_calibration/*
00559 """
00560     replaced_str = replaceStringWithDict(file_str, {"root_dir": root_dir, "port": port})
00561     with open(os.path.join(root_dir, "calibrate_staro.sh"), "w") as f:
00562         f.write(replaced_str)
00563     os.chmod(os.path.join(root_dir, "calibrate_staro.sh"), 0755)
00564         
00565 def copyBagFiles(root_dir):
00566     os.mkdir(os.path.join(root_dir, "staro_calibration"))
00567     to_path = os.path.join(root_dir, "staro_calibration", "cal_measurements.bag")
00568     shutil.copyfile("cal_measurements.bag", to_path)
00569 
00570 def genFiles(estimation_config_include_leg,
00571              estimation_config_use_chain,
00572              estimation_config_calibrate_joint,
00573              free_cb_locations_use_leg,
00574              free_cameras_include_leg,
00575              free_cameras_include_arm,
00576              free_arms_use_arm,
00577              free_arms_use_head,
00578              free_arms_use_chest,
00579              free_arms_use_leg):
00580     directory_name = "test_%s_%s_%s_%s_%s_%s_%s_%s_%s_%s" % (estimation_config_include_leg,
00581                                                              estimation_config_use_chain,
00582                                                              estimation_config_calibrate_joint,
00583                                                              free_cb_locations_use_leg,
00584                                                              free_cameras_include_leg,
00585                                                              free_cameras_include_arm,
00586                                                              free_arms_use_arm,
00587                                                              free_arms_use_head,
00588                                                              free_arms_use_chest,
00589                                                              free_arms_use_leg)
00590     if "--cleanup" in sys.argv:
00591         if os.path.exists(directory_name):
00592             shutil.rmtree(directory_name)
00593     os.mkdir(directory_name)
00594     generateEstimationConfig(directory_name, 
00595                              estimation_config_include_leg,
00596                              estimation_config_use_chain,
00597                              estimation_config_calibrate_joint)
00598     generateFreeCBLocations(directory_name, free_cb_locations_use_leg)
00599     generateFreeCameras(directory_name, free_cameras_include_leg,
00600                         free_cameras_include_arm)
00601     generateFreeArms(directory_name,
00602                      free_arms_use_arm,
00603                      free_arms_use_head,
00604                      free_arms_use_chest,
00605                      free_arms_use_leg)
00606     generateSystemYaml(directory_name)
00607     generateSh(directory_name)
00608     copyBagFiles(directory_name)
00609 
00610 
00611 def multipleExecution():
00612     parallel_num = 32
00613     # find all calibrate_staro.sh
00614     sh_files = []
00615     free_workers = [None] * parallel_num
00616     for root, dirs, files in os.walk('.'):
00617         if "calibrate_staro.sh" in files and "latest_calibrated_xml" not in files:
00618             sh_files.append(os.path.join(root, "calibrate_staro.sh"))
00619             del dirs[:]
00620     while len(sh_files) > 0:
00621         for p, i in zip(free_workers, range(len(free_workers))):
00622             if p == None or p.poll() != None:
00623                 # we can use the worker
00624                 new_env = os.environ.copy()
00625                 new_env["ROS_MASTER_URI"] = "http://localhost:%d" % (11311 + i + 1)
00626                 process = subprocess.Popen([sh_files.pop()], env=new_env, shell=True)
00627                 free_workers[i] = process
00628         print "%d tasks are remained"  % (len(sh_files))
00629         print free_workers
00630         time.sleep(1)
00631 
00632         
00633 
00634 def main():
00635     if "--execute" in sys.argv:
00636         multipleExecution()
00637         return
00638     for estimation_config_include_leg in (False, True):
00639         for estimation_config_use_chain in (False, True):
00640             for estimation_config_calibrate_joint in (False, True):
00641                 for free_cb_locations_use_leg in (False, True):
00642                     for free_cameras_include_leg in (False, True):
00643                         for free_cameras_include_arm in (False, True):
00644                             for free_arms_use_arm in (False, True):
00645                                 for free_arms_use_head in (False, True):
00646                                     for free_arms_use_chest in (False, True):
00647                                         for free_arms_use_leg in (False, True):
00648                                             genFiles(estimation_config_include_leg,
00649                                                      estimation_config_use_chain,
00650                                                      estimation_config_calibrate_joint,
00651                                                      free_cb_locations_use_leg,
00652                                                      free_cameras_include_leg,
00653                                                      free_cameras_include_arm,
00654                                                      free_arms_use_arm,
00655                                                      free_arms_use_head,
00656                                                      free_arms_use_chest,
00657                                                      free_arms_use_leg)
00658 
00659 if __name__ == "__main__":
00660     main()


jsk_calibration
Author(s):
autogenerated on Wed Jul 19 2017 02:54:21