collect_stereo_data.py
Go to the documentation of this file.00001 #!/usr/bin/env python 00002 ################################################################# 00003 ##\file 00004 # 00005 # \note 00006 # Copyright (c) 2011-2012 \n 00007 # Fraunhofer Institute for Manufacturing Engineering 00008 # and Automation (IPA) \n\n 00009 # 00010 ################################################################# 00011 # 00012 # \note 00013 # Project name: care-o-bot 00014 # \note 00015 # ROS stack name: cob_calibration 00016 # \note 00017 # ROS package name: cob_calibration_executive 00018 # 00019 # \author 00020 # Author: Sebastian Haug, email:sebhaug@gmail.com 00021 # \author 00022 # Supervised by: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de 00023 # 00024 # \date Date of creation: January 2012 00025 # 00026 ################################################################# 00027 # 00028 # Redistribution and use in source and binary forms, with or without 00029 # modification, are permitted provided that the following conditions are met: 00030 # 00031 # - Redistributions of source code must retain the above copyright 00032 # notice, this list of conditions and the following disclaimer. \n 00033 # - Redistributions in binary form must reproduce the above copyright 00034 # notice, this list of conditions and the following disclaimer in the 00035 # documentation and/or other materials provided with the distribution. \n 00036 # - Neither the name of the Fraunhofer Institute for Manufacturing 00037 # Engineering and Automation (IPA) nor the names of its 00038 # contributors may be used to endorse or promote products derived from 00039 # this software without specific prior written permission. \n 00040 # 00041 # This program is free software: you can redistribute it and/or modify 00042 # it under the terms of the GNU Lesser General Public License LGPL as 00043 # published by the Free Software Foundation, either version 3 of the 00044 # License, or (at your option) any later version. 00045 # 00046 # This program is distributed in the hope that it will be useful, 00047 # but WITHOUT ANY WARRANTY; without even the implied warranty of 00048 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00049 # GNU Lesser General Public License LGPL for more details. 00050 # 00051 # You should have received a copy of the GNU Lesser General Public 00052 # License LGPL along with this program. 00053 # If not, see <http://www.gnu.org/licenses/>. 00054 # 00055 ################################################################# 00056 PKG = 'cob_calibration_executive' 00057 NODE = 'collect_stereo_data_node' 00058 import roslib; roslib.load_manifest(PKG) 00059 import rospy 00060 00061 from simple_script_server import simple_script_server 00062 from cob_calibration_srvs.srv import Capture 00063 00064 def main(): 00065 rospy.init_node(NODE) 00066 print "==> %s started " % NODE 00067 00068 # service client 00069 image_capture_service_name = "/image_capture/capture" 00070 capture = rospy.ServiceProxy(image_capture_service_name, Capture) 00071 rospy.wait_for_service(image_capture_service_name, 1) 00072 print "--> service client for capture images initialized" 00073 00074 # init 00075 print "--> initializing sss" 00076 sss = simple_script_server() 00077 sss.init("base") 00078 sss.init("torso") 00079 sss.init("head") 00080 sss.recover("base") 00081 sss.recover("torso") 00082 sss.recover("head") 00083 00084 print "--> setup care-o-bot for capture" 00085 sss.move("head", "back") 00086 sss.move("torso", "home") 00087 00088 print "==> capturing images" 00089 positions = rospy.get_param("/script_server/arm/all_intrinsic") 00090 for pos in positions: 00091 print "--> moving arm to position '%s'" % pos 00092 sss.move("arm", pos) 00093 sss.sleep(1.5) 00094 print "--> capturing '%s' sample" % pos 00095 res = capture() 00096 if not res: print "--> ERROR during capture, skipping sample..." 00097 00098 print "==> capturing finished, moving are to 'calibration' position" 00099 sss.move("arm", 'calibration') 00100 00101 if __name__ == '__main__': 00102 main() 00103 print "==> done exiting"