Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('camera_pose_calibration')
00004 import rospy
00005 import actionlib
00006 import sys
00007
00008 from image_cb_detector.msg import *
00009
00010
00011 def usage():
00012 print "Usage:"
00013 print " start_cb_detector_action_old.py num_x num_y spacing [image_scaling]"
00014
00015
00016 if __name__ == '__main__':
00017 rospy.init_node('start_checkerboard_detector')
00018
00019 if len(rospy.myargv()) < 4:
00020 usage()
00021 sys.exit(-1)
00022 elif len(rospy.myargv()) < 5:
00023 scaling = 1.0
00024 else:
00025 scaling = rospy.myargv()[4]
00026
00027 print "Configuring checkerboard size %sx%s, checker size %s"%(rospy.myargv()[1], rospy.myargv()[2], rospy.myargv()[3])
00028
00029 client = actionlib.SimpleActionClient('cb_detector_config', ConfigAction)
00030 print "Waiting for Server"
00031 client.wait_for_server()
00032 print "Found Server"
00033
00034 goal = ConfigGoal()
00035
00036 goal.num_x = int(rospy.myargv()[1])
00037 goal.num_y = int(rospy.myargv()[2])
00038 goal.spacing_x = float(rospy.myargv()[3])
00039 goal.spacing_y = float(rospy.myargv()[3])
00040 goal.width_scaling = scaling
00041 goal.height_scaling = scaling
00042 goal.subpixel_window = 5
00043 goal.subpixel_zero_zone = 1
00044 client.send_goal_and_wait(goal)
00045 print "Done sending goal"