def test_checkerboard_object_points_4x4(self): c = Checkerboard((4,4), 1) points = c.get_pattern_points() points_ = [[0, 0, 0], [1, 0, 0], [2, 0, 0], [3, 0, 0], [0, 1, 0], [1, 1, 0], [2, 1, 0], [3, 1, 0], [0, 2, 0], [1, 2, 0], [2, 2, 0], [3, 2, 0], [0, 3, 0], [1, 3, 0], [2, 3, 0], [3, 3, 0]] self.assertTrue(np.allclose(points, points_))
def __init__(self): rospy.init_node(NODE) pattern = rospy.get_param('~calibration_pattern') self.cameras = rospy.get_param('~cameras') pattern_string = pattern["pattern_size"].split("x") pattern_size = (int(pattern_string[0]), int(pattern_string[1])) cb = Checkerboard(pattern_size, pattern["square_size"]) self.detector = CheckerboardDetector(cb) self.bridge = CvBridge() self.pub = rospy.Publisher("cb_detections", MarkerArray) self.topics = [self.cameras["reference"]["topic"]] for camera in self.cameras["further"]: self.topics.append(camera["topic"]) self.info_topics = [ t.replace("image_raw", "camera_info") for t in self.topics ] self.info_topics = [ t.replace("image_color", "camera_info") for t in self.info_topics ] print self.topics print self.info_topics self.images = dict.fromkeys(self.topics) self.camera_infos = dict.fromkeys(self.topics) self.colors = [ColorRGBA() for i in range(len(self.topics))] hue_values = np.linspace(0, 1, len(self.topics) + 1) for c, hue, topic in zip(self.colors, hue_values, self.topics): c.a = 1 (c.r, c.g, c.b) = hsv_to_rgb(hue, 1, 1) print "Listening to: %s - Color RGB: (%d, %d, %d)" % ( topic, c.r * 255, c.g * 255, c.b * 255) self.points = [] for point in cb.get_pattern_points(): p = Point() p.x = point[0] p.y = point[1] p.z = point[2] self.points.append(p) for topic, info_topic in zip(self.topics, self.info_topics): rospy.Subscriber(topic, Image, self.callback_image, topic) rospy.Subscriber(info_topic, CameraInfo, self.callback_info, topic) rospy.sleep(3)
def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' rospy.init_node(NODE) print "==> started " + NODE # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split("x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() self.ts = TorsoState()
def run_mono_calibration(self): ''' Runs the calibration ''' print "==> starting monocular calibration" # set up Checkerboard, CheckerboardDetector and MonoCalibrator board = Checkerboard(self.pattern_size, self.square_size) detector = CheckerboardDetector(board) calibrator = MonoCalibrator(board, detector, self.folder, self.image_prefix) # run calibration (rms, camera_matrix, projection_matrix, dist_coeffs, (h, w), _, _) = calibrator.calibrate_monocular_camera(self.alpha) print "==> successfully calibrated, reprojection RMS (in pixels):", rms # create CalibrationData object with results camera_info = CalibrationData(self.camera_name, self.frame_id, w, h) camera_info.camera_matrix = camera_matrix camera_info.distortion_coefficients = dist_coeffs camera_info.projection_matrix = projection_matrix camera_info.dist_coeffs = dist_coeffs # save results camera_info.save_camera_yaml_file(self.output_file) print "==> saved results to:", self.output_file # verbose mode if self.verbose: print "--> results:" np.set_printoptions(suppress=1) print "camera matrix:\n", camera_matrix print "distortion coefficients:\n", dist_coeffs print "projection matrix:\n", projection_matrix
def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split("x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() topic_name = '/stereo/left/' # subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients rospy.Subscriber(topic_name + 'camera_info', CameraInfo, self.__camera_info_callback__) # subscribe to /cam3d/rgb/image_raw for image data rospy.Subscriber(topic_name + 'image_color', Image, self.__image_raw_callback__) # wait until camera informations are recieved. start_time = rospy.Time.now() while not (self.camera_info_received or rospy.is_shutdown()): rospy.sleep(0.05) if start_time + rospy.Duration(2.0) < rospy.Time.now(): # print warning every 2 seconds if the message is stil missing print "--> still waiting for /cam3d/rgb/camera_info" start_time = rospy.Time.now() # convert camera matrix an distortion coefficients and store them camera_matrix = self.camera_info.K cm = np.asarray(camera_matrix) self.cm = np.reshape(cm, (3, 3)) dist_coeffs = self.camera_info.D self.dc = np.asarray(dist_coeffs) self.frame = self.camera_info.header.frame_id
def test_checkerboard_object_points_4x4(self): c = Checkerboard((4, 4), 1) points = c.get_pattern_points() points_ = [ [0, 0, 0], [1, 0, 0], [2, 0, 0], [3, 0, 0], [0, 1, 0], [1, 1, 0], [2, 1, 0], [3, 1, 0], [0, 2, 0], [1, 2, 0], [2, 2, 0], [3, 2, 0], [0, 3, 0], [1, 3, 0], [2, 3, 0], [3, 3, 0], ] self.assertTrue(np.allclose(points, points_))
def get_corners(self): # -------------------- # receive images # ----------------- for v in self._images_received: self._images_received[v] = False start_time = rospy.Time.now() while (not all(self._images_received.values()) or not all(self.transformations.values())): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still # missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): for name, v in self._images_received.iteritems(): if not v: print "--> still waiting for sample from %s" % name for name, v in self._transformations_received.iteritems(): if not v: print "--> still waiting for sample from %s" % name start_time = rospy.Time.now() # set up checkerboard and checkerboard detector checkerboard = Checkerboard(pattern_size, square_size) checkerboard_detector = CheckerboardDetector(checkerboard) # detect cb for camera, image in self._images.iteritems(): image = image["image"] cvImage = self.bridge.imgmsg_to_cv2(image, "mono8") #imagecv = cv2util.cvmat2np(cvImage) self._images[camera]['corners'] = None try: corners = checkerboard_detector.detect_image_points( cvImage, is_grayscale=True) except: rospy.logwarn("No calibration pattern found for: '%s'" % camera) else: print "cb found: %s" % camera #img_points = [] #for (x, y) in corners.reshape(-1, 2): # img_points.append(ImagePoint(x, y)) self._images[camera]['corners'] = corners return self._images
def test_detect_image_points_color_image(self): # create Checkerboard instance and read image c = Checkerboard((9, 6), 0.03) image = cv2.imread(CHECKERBOARD_IMAGE, 0) # create CheckerboardDetector instance and detect image points cd = CheckerboardDetector(c) points = cd.detect_image_points(image, True) #dump = {'points': points.flatten().tolist()} #yaml.dump(dump, open(CHECKERBOARD_IMAGE_EXPECTED_POINTS_YAML, "w")) # load expected points points_expected = yaml.load( file(CHECKERBOARD_IMAGE_EXPECTED_POINTS_YAML))['points'] self.assertTrue(np.allclose(points.flatten().tolist(), points_expected))
def __init__(self): ''' Initializes storage, gets parameters from parameter server and logs to rosinfo ''' self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard( (9, 6), 0.03) ### Take the parameters from the parameter server self.detector = CheckerboardDetector(self.board) rospy.init_node(NODE) self.counter = 0 # Get params from ros parameter server or use default self.cams = rospy.get_param("~cameras") self.camera = [self.cams["reference"]["topic"]] if self.cams.has_key("further"): rospy.loginfo("FURTHER cameras specified") for cam in self.cams["further"]: self.camera.append(cam["topic"]) else: rospy.logwarn("No FURTHER cameras specified") self.numCams = len(self.camera) # Init images self.image = [] self.image_received = [False] * self.numCams for id in range(self.numCams): self.image.append( Image() ) # wait for a message from each camera this ensures that all cameras are working # Subscribe to images self.imageSub = [] for id in range(self.numCams): self.imageSub.append( rospy.Subscriber(self.camera[id], Image, self._imageCallback, id)) # Wait for image messages for id in range(self.numCams): print "now checking for incoming frames on the %s topic" % self.camera[ id] # pdb.set_trace() rospy.wait_for_message(self.camera[id], Image, 5)
def test_calculate_object_pose(self): # create Checkerboard instance and read image c = Checkerboard((9, 6), 0.03) image = cv2.imread(CHECKERBOARD_IMAGE, 0) # create CheckerboardDetector instance and detect object pose # (only dummy matrices, does not correspond to real camera used) cd = CheckerboardDetector(c) camera_matrix = np.matrix([1200, 0, 600, 0, 1200, 600, 0, 0, 1], dtype=np.float32).reshape((3, 3)) dist_coeffs = np.matrix([0, 0, 0, 0, 0], dtype=np.float32).reshape( (1, 5)) (rvec, tvec) = cd.calculate_object_pose(image, camera_matrix, dist_coeffs, True) rvec_expected = np.matrix([[0.99905897, 0.035207, -0.02533079], [-0.0208742, 0.90224111, 0.43072642], [0.03801906, -0.42979234, 0.90212699]]) tvec_expected = np.matrix([[-0.03181351], [-0.13593217], [0.7021291]]) self.assertTrue(np.allclose(rvec_expected, rvec)) self.assertTrue(np.allclose(tvec_expected, tvec))
def run_stereo_calibration(self): ''' Runs the calibration ''' # set up Checkerboard, CheckerboardDetector and MonoCalibrator board = Checkerboard(self.pattern_size, self.square_size) detector = CheckerboardDetector(board) calibrator = StereoCalibrator(board, detector, self.folder, self.image_prefix_l, self.image_prefix_r) # run calibration ((rms_l, rms_r, rms_stereo), camera_matrix_l, dist_coeffs_l, rectification_matrix_l, projection_matrix_l, camera_matrix_r, dist_coeffs_r, rectification_matrix_r, projection_matrix_r, (h, w), R, T) = calibrator.calibrate_stereo_camera(self.alpha) print "==> successfully calibrated, stereo reprojection RMS (in pixels):", rms_stereo # create CalibrationData object with results camera_info_l = CalibrationData(self.camera_name_l, self.frame_id_l, w, h) camera_info_l.camera_matrix = camera_matrix_l camera_info_l.rectification_matrix = rectification_matrix_l camera_info_l.projection_matrix = projection_matrix_l camera_info_l.distortion_coefficients = dist_coeffs_l camera_info_r = CalibrationData(self.camera_name_r, self.frame_id_r, w, h) camera_info_r.camera_matrix = camera_matrix_r camera_info_r.rectification_matrix = rectification_matrix_r camera_info_r.projection_matrix = projection_matrix_r camera_info_r.distortion_coefficients = dist_coeffs_r # save results camera_info_l.save_camera_yaml_file(self.output_file_l) print "==> saved left results to:", self.output_file_l camera_info_r.save_camera_yaml_file(self.output_file_r) print "==> saved right results to:", self.output_file_r # convert baseline (invert transfrom as T and R bring right frame into left # and we need transform from left to right for urdf!) M = np.matrix(np.vstack((np.hstack( (R, T)), [0.0, 0.0, 0.0, 1.0]))) # 4x4 homogeneous matrix M_inv = M.I T_inv = np.array(M_inv[:3, 3]).flatten().tolist() # T as list (x, y, z) R_inv = list(tf.transformations.euler_from_matrix( M_inv[:3, :3])) # convert R to (roll, pitch, yaw) # save baseline if (self.calibration_urdf_in != "" and self.calibration_urdf_out != ""): attributes2update = { self.baseline_prop_prefix + 'x': T_inv[0], self.baseline_prop_prefix + 'y': T_inv[1], self.baseline_prop_prefix + 'z': T_inv[2], self.baseline_prop_prefix + 'roll': R_inv[0], self.baseline_prop_prefix + 'pitch': R_inv[1], self.baseline_prop_prefix + 'yaw': R_inv[2] } urdf_updater = CalibrationUrdfUpdater(self.calibration_urdf_in, self.calibration_urdf_out, self.verbose) urdf_updater.update(attributes2update) print "==> updated baseline in:", self.calibration_urdf_out else: print "==> NOT saving baseline to urdf file! Parameters 'calibration_urdf_in' and/or 'calibration_urdf_out' are empty..." # verbose mode if self.verbose: print "--> results:" np.set_printoptions(suppress=1) print "left\n----" print "rms left monocular calibration:", rms_l print "camera matrix:\n", camera_matrix_l print "distortion coefficients:\n", dist_coeffs_l print "rectification matrix:\n", rectification_matrix_l print "projection matrix:\n", projection_matrix_l print print "right\n-----" print "rms right monocular calibration:", rms_r print "camera matrix:\n", camera_matrix_r print "distortion coefficients:\n", dist_coeffs_r print "rectification matrix:\n", rectification_matrix_r print "projection matrix:\n", projection_matrix_r print print "baseline (transform from left to right camera)\n--------" print "R (in rpy):\n", R_inv print "T (in xyz):\n", T_inv
def test_checkerboard_object_points_2x2_scale(self): c = Checkerboard((2, 2), 1.5) points = c.get_pattern_points() points_ = [[0, 0, 0], [1.5, 0, 0], [0, 1.5, 0], [1.5, 1.5, 0]] self.assertTrue(np.allclose(points, points_))
def test_checkerboard_object_points_2x2_scale(self): c = Checkerboard((2,2), 1.5) points = c.get_pattern_points() points_ = [[0, 0, 0], [1.5, 0, 0], [0, 1.5, 0], [1.5, 1.5, 0]] self.assertTrue(np.allclose(points, points_))
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size): ''' Main capturing function. Gets a set of recent messages for all needed topics. Processes messages and creates RobotMeasuerment message which is published. @param sample_id: Sample identifier (e.g. sample01) @type sample_id: string @param target_id: Name of checkerboard (e.g. cb_9x6) @type target_id: string @param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain) @type chain_id: string @param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6)) @type pattern_size: tuple(x, y) @param square_size: Size of checkerboard sqaures (im m) @type square_size: float ''' # capture measurements # -------------------- # create robot measurement msg and publish # ----------------- robot_msg = RobotMeasurement() robot_msg.sample_id = sample_id robot_msg.target_id = target_id robot_msg.chain_id = chain_id # -------------------- # receive images # ----------------- for v in self._images_received: self._images_received[v] = False for v in self.transformations_received: self.transformations_received[v] = False print self._images_received start_time = rospy.Time.now() while (not all(self._images_received.values()) or not all(self.transformations.values())): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still # missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): for name, v in self._images_received.iteritems(): if not v: print "--> still waiting for sample from %s" % name for name, v in self._transformations_received.iteritems(): if not v: print "--> still waiting for sample from %s" % name start_time = rospy.Time.now() print "got sample" #latest_left = self._left # set up checkerboard and checkerboard detector # --------------------------------------------- checkerboard = Checkerboard(pattern_size, square_size) checkerboard_detector = CheckerboardDetector(checkerboard) # detect cb # -------------- for name, image in self._images.iteritems(): image = image["image"] #print image.header cvImage = self.bridge.imgmsg_to_cv(image, "mono8") imagecv = cv2util.cvmat2np(cvImage) try: corners = checkerboard_detector.detect_image_points( imagecv, is_grayscale=True) except: # cb not found rospy.logwarn("No calibration pattern found for: '%s'" % name) return False else: print "cb found: %s" % name img_points = [] for (x, y) in corners.reshape(-1, 2): img_points.append(ImagePoint(x, y)) # create camera msg left # ---------------------- cam_msg = CameraMeasurement() cam_msg.camera_id = name #print "crash before" cam_msg.header.stamp = image.header.stamp #print "crash after" #cam_msg.cam_info = image["camera_info"] cam_msg.image_points = img_points cam_msg.verbose = False robot_msg.M_cam.append(cam_msg) cam_msg.image = image # print cam_msg.camera_id #print cam_msg.header # cam_msg.image_rect = latest_left["image_rect"] # cam_msg.features = # Not implemented here #---------------------- #DEBUG publish pic #----------------- #self._image_pub_left.publish(latest_left["image"]) #---------------------- # Fill robot_msg #---------------------- robot_msg.M_chain = self.transformations.values() self._robot_measurement_pub.publish(robot_msg) return True
def run_stereo_calibration(self): ''' Runs the calibration ''' # set up Checkerboard, CheckerboardDetector and MonoCalibrator board = Checkerboard(self.pattern_size, self.pattern["square_size"]) detector = CheckerboardDetector(board) attributes2update = {} output = "" camera_ref_saved = False; for image_prefix_dep,\ camera_name_dep,\ frame_id_dep,\ output_file_dep,\ baseline_prop_prefix,\ is_part_of_stereo_system in zip(self.image_prefixes_dep, self.camera_names_dep, self.frame_ids_dep, self.output_files_dep, self.baseline_prop_prefixes_dep, self.is_part_of_stereo_system): if image_prefix_dep is not None: print "Calibrating %s: \n\t Frame: %s \n\t Output File: %s \n\t Baseline Prefix: %s"%(camera_name_dep,frame_id_dep,output_file_dep, baseline_prop_prefix) output += self.camera_name_ref +" -> " + camera_name_dep + ": \n" calibrator = StereoCalibrator(board, detector, self.folder, self.image_prefix_ref, image_prefix_dep) # run calibration for stereo pair ( (rms_ref, rms_dep, rms_stereo), camera_matrix_ref, dist_coeffs_ref, rectification_matrix_ref, projection_matrix_ref, camera_matrix_dep, dist_coeffs_dep, rectification_matrix_dep, projection_matrix_dep, ((h_ref, w_ref), (h_dep, w_dep)), R, T) = calibrator.calibrate_stereo_camera(self.alpha) output += "==> successfully calibrated, stereo reprojection RMS (in pixels): " output += str(rms_stereo) output += "\n\n" # create CalibrationData object with results camera_info_ref = CalibrationData( self.camera_name_ref, self.frame_id_ref, w_ref, h_ref) camera_info_ref.camera_matrix = camera_matrix_ref camera_info_ref.rectification_matrix = rectification_matrix_ref camera_info_ref.projection_matrix = projection_matrix_ref camera_info_ref.distortion_coefficients = dist_coeffs_ref camera_info_dep = CalibrationData( camera_name_dep, frame_id_dep, w_dep, h_dep) camera_info_dep.camera_matrix = camera_matrix_dep camera_info_dep.rectification_matrix = rectification_matrix_dep camera_info_dep.projection_matrix = projection_matrix_dep camera_info_dep.distortion_coefficients = dist_coeffs_dep if output_file_dep is not None: if not is_part_of_stereo_system: # TODO: compute new projectionmatrix (projection_matrix, _) = cv2.getOptimalNewCameraMatrix( camera_matrix_dep, dist_coeffs_dep, (w_dep, h_dep),0 ) camera_info_dep.projection_matrix = np.array(np.hstack(( projection_matrix, np.matrix([0, 0, 0]).reshape(3, 1)))) camera_info_dep.rectification_matrix=[1,0,0, 0,1,0, 0,0,1] # save results if not camera_ref_saved: camera_info_ref.save_camera_yaml_file(self.output_file_ref) print "==> saved left results to:", self.output_file_ref camera_ref_saved = True #output += "==> saved left results to:", self.output_file_ref camera_info_dep.save_camera_yaml_file(output_file_dep) print "==> saved " + camera_name_dep + " results to:", output_file_dep #output += "==> saved " + camera_name_dep + " results to:", output_file_dep # convert baseline (invert transfrom as T and R bring right frame into left # and we need transform from left to right for urdf!) M = np.matrix(np.vstack((np.hstack( (R, T)), [0.0, 0.0, 0.0, 1.0]))) # 4x4 homogeneous matrix # TODO: tests with real hardware samples # compute transformation from reference to parent frame of camera # k_u = kinematic_utils() # k_u.get_parent(frame_id_dep) # M_parent = np.matrix(k_u.get_tf_to_parent(self.frame_id_ref)) # # resulting transformation M_inv = M.I # M_result = M_inv * M_parent # # urdf specifies origin -> inverse # M_inv = M_result.I T_inv = np.array( M_inv[:3, 3]).flatten().tolist() # T as list (x, y, z) R_inv = list(tf.transformations.euler_from_matrix( M_inv[:3, :3])) # convert R to (roll, pitch, yaw) baseline_prop_prefix = 'offset_' + baseline_prop_prefix attributes2update[baseline_prop_prefix + 'x'] = T_inv[0] attributes2update[baseline_prop_prefix + 'y'] = T_inv[1] attributes2update[baseline_prop_prefix + 'z'] = T_inv[2] attributes2update[baseline_prop_prefix + 'roll'] = R_inv[0] attributes2update[baseline_prop_prefix + 'pitch'] = R_inv[1] attributes2update[baseline_prop_prefix + 'yaw'] = R_inv[2] #verbose mode if self.verbose: print "--> results:" np.set_printoptions(suppress=1) print "left\n----" print "rms left monocular calibration:", rms_ref print "camera matrix:\n", camera_matrix_ref print "distortion coefficients:\n", dist_coeffs_ref print "rectification matrix:\n", rectification_matrix_ref print "projection matrix:\n", projection_matrix_ref print print "right\n-----" print "rms right monocular calibration:", rms_dep print "camera matrix:\n", camera_matrix_dep print "distortion coefficients:\n", dist_coeffs_dep print "rectification matrix:\n", rectification_matrix_dep print "projection matrix:\n", projection_matrix_dep print print "baseline (transform from left to right camera)\n--------" print "R (in rpy):\n", R_inv print "T (in xyz):\n", T_inv # save baseline if (self.calibration_offset_urdf != "" and self.calibration_default_urdf != ""): for p in attributes2update.iteritems(): print "%s: %s"%p urdf_updater = CalibrationUrdfUpdater(self.calibration_offset_urdf, self.calibration_offset_urdf, debug = self.verbose, urdf_default=self.calibration_default_urdf) urdf_updater.update(attributes2update) print "==> updated baseline in:", self.calibration_offset_urdf else: print "==> NOT saving baseline to urdf file! Parameters 'calibration_urdf_in' and/or 'calibration_urdf_out' are empty..." print output
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size): ''' Main capturing function. Gets a set of recent messages for all needed topics. Processes messages and creates RobotMeasuerment message which is published. @param sample_id: Sample identifier (e.g. sample01) @type sample_id: string @param target_id: Name of checkerboard (e.g. cb_9x6) @type target_id: string @param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain) @type chain_id: string @param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6)) @type pattern_size: tuple(x, y) @param square_size: Size of checkerboard sqaures (im m) @type square_size: float ''' # capture measurements # -------------------- self._left_received = False self._right_received = False self._kinect_rgb_received = False start_time = rospy.Time.now() while (not self._left_received or not self._right_received or not self._kinect_rgb_received): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): if not self._left_received: print "--> still waiting for sample from left" if not self._right_received: print "--> still waiting for sample from right" if not self._kinect_rgb_received: print "--> still waiting for sample from kinect" start_time = rospy.Time.now() latest_left = self._left latest_right = self._right latest_kinect_rgb = self._kinect_rgb self._torso_joint_msg_received = False self._arm_joint_msg_received = False start_time = rospy.Time.now() while (not self._torso_joint_msg_received or not self._arm_joint_msg_received): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): if not self._torso_joint_msg_received: print "--> still waiting for torso joint states" if not self._arm_joint_msg_received: print "--> still waiting for srm joint states" start_time = rospy.Time.now() latest_torso = self._torso_joint_msg latest_arm = self._arm_joint_msg # set up checkerboard and checkerboard detector # --------------------------------------------- checkerboard = Checkerboard(pattern_size, square_size) checkerboard_detector = CheckerboardDetector(checkerboard) # detect cb left # -------------- cvImage = self.bridge.imgmsg_to_cv(latest_left["image_rect"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: left" img_points_left = [] for (x, y) in corners.reshape(-1, 2): img_points_left.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg left # ---------------------- cam_msg_left = CameraMeasurement() cam_msg_left.camera_id = "left" cam_msg_left.header.stamp = latest_left["camera_info"].header.stamp cam_msg_left.cam_info = latest_left["camera_info"] cam_msg_left.image_points = img_points_left cam_msg_left.verbose = False #cam_ms_leftg.image = latest_left["image_color"] #cam_msg_left.image_rect = latest_left["image_rect"] #cam_msg_left.features = # Not implemented here # detect cb right # -------------- cvImage = self.bridge.imgmsg_to_cv(latest_right["image_rect"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: right" img_points_right = [] for (x, y) in corners.reshape(-1, 2): img_points_right.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg right # ----------------------- cam_msg_right = CameraMeasurement() cam_msg_right.camera_id = "right" cam_msg_right.header.stamp = latest_right["camera_info"].header.stamp cam_msg_right.cam_info = latest_right["camera_info"] cam_msg_right.image_points = img_points_right cam_msg_right.verbose = False #cam_msg_right.image = latest_right["image_color"] #cam_msg_right.image_rect = latest_right["image_rect"] #cam_msg_right.features = # Not implemented here # detect cb kinect_rgb # -------------------- cvImage = self.bridge.imgmsg_to_cv(latest_kinect_rgb["image_color"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: kinect_rgb" img_points_kinect_rgb = [] for (x, y) in corners.reshape(-1, 2): img_points_kinect_rgb.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg kinect_rgb # ---------------------------- cam_msg_kinect_rgb = CameraMeasurement() cam_msg_kinect_rgb.camera_id = "kinect_rgb" cam_msg_kinect_rgb.header.stamp = latest_kinect_rgb[ "camera_info"].header.stamp cam_msg_kinect_rgb.cam_info = latest_kinect_rgb["camera_info"] cam_msg_kinect_rgb.image_points = img_points_kinect_rgb cam_msg_kinect_rgb.verbose = False #cam_ms_kinect_rgbg.image = latest_kinect_rgb["image_color"] #cam_msg_kinect_rgb.image_rect = latest_kinect_rgb["image_rect"] #cam_msg_kinect_rgb.features = # Not implemented here # create torso_chain msg # ---------------------- torso_chain_msg = ChainMeasurement() torso_chain_msg.header = latest_torso.header torso_chain_msg.chain_id = "torso_chain" torso_chain_msg.chain_state = latest_torso # create arm_chain msg # -------------------- arm_chain_msg = ChainMeasurement() arm_chain_msg.header = latest_arm.header arm_chain_msg.chain_id = "arm_chain" arm_chain_msg.chain_state = latest_arm # DEBUG publish pic # ----------------- self._image_pub_left.publish(latest_left["image_color"]) self._image_pub_right.publish(latest_right["image_color"]) self._image_pub_kinect_rgb.publish(latest_kinect_rgb["image_color"]) # create robot measurement msg and publish # ----------------- robot_msg = RobotMeasurement() robot_msg.sample_id = sample_id robot_msg.target_id = target_id robot_msg.chain_id = chain_id robot_msg.M_cam = [cam_msg_left, cam_msg_right, cam_msg_kinect_rgb] robot_msg.M_chain = [torso_chain_msg, arm_chain_msg] self._robot_measurement_pub.publish(robot_msg) return True