Esempio n. 1
0
 def handle_monocular(self, msg):
     if self.c == None:
         if self._camera_name:
             self.c = MonoCalibrator(self._boards,
                                     self._calib_flags,
                                     self._pattern,
                                     name=self._camera_name)
         else:
             self.c = MonoCalibrator(self._boards, self._calib_flags,
                                     self._pattern)
     if self.min_samples_number >= self.captured_images_count and self.c.goodenough:
         self.enough_samples = True
     if not self.capture_img_permission and not self.enough_samples:
         self.permission_query()
     if self.capture_img_permission and not self.image_captured:
         # This should just call the MonoCalibrator
         drawable = self.c.handle_msg(msg)
         self.displaywidth = drawable.scrib.cols
         #self.redraw_monocular(drawable)
         if len(self.c.db) > self.captured_images_count:
             print 'len(self.c.db): %s    pic_count: %s' % (len(
                 self.c.db), self.captured_images_count)
             print 'Image captured'
             self.captured_images_count += 1
             self.capture_img_permission = False
             self.image_captured = True
Esempio n. 2
0
    def handle_monocular(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
                                        checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern,
                                        checkerboard_flags=self.checkerboard_flags)

        # This should just call the MonoCalibrator
        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.scrib.shape[1]
        self.redraw_monocular(drawable)

#        print "drawable_scrib"
          
        pub = rospy.Publisher('goodenoughTopic', Bool, queue_size=10)
        hello_str = self.c.goodenough
        rospy.loginfo(hello_str)
        pub.publish(hello_str)

        bridge = cv_bridge.CvBridge()
        pub1 = rospy.Publisher('boardImage', sensor_msgs.msg.Image, queue_size=10)
        pub1.publish(bridge.cv2_to_imgmsg(drawable.scrib, "bgr8"))

        #calibrate picture enough, must stop thread
        if self.c.goodenough:
            self.mth.stop()
Esempio n. 3
0
    def handle_stereo(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
                                          checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern,
                                          checkerboard_flags=self._checkerboard_flags)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
        self.redraw_stereo(drawable)
    def handle_monocular(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
                                        checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern,
                                        checkerboard_flags=self.checkerboard_flags)

        # This should just call the MonoCalibrator
        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.scrib.shape[1]
        self.redraw_monocular(drawable)
Esempio n. 5
0
    def handle_stereo(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = StereoCalibrator(self._boards,
                                          self._calib_flags,
                                          self._pattern,
                                          name=self._camera_name)
            else:
                self.c = StereoCalibrator(self._boards, self._calib_flags,
                                          self._pattern)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.cols + drawable.rscrib.cols
        self.redraw_stereo(drawable)
Esempio n. 6
0
    def handle_stereo(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = StereoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    name=self._camera_name,
                    checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = StereoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    checkerboard_flags=self._checkerboard_flags)

        (lmsg, rmsg, vmsg) = msg
        (drawable, is_good_sample) = self.c.handle_msg((lmsg, rmsg))
        self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
        self.redraw_stereo(drawable)

        if is_good_sample:
            print "Saving good sample %d" % self.save_counter

            limg = self.cv_bridge.imgmsg_to_cv2(lmsg, "bgr8")
            cv2.imwrite("left_%02d.png" % self.save_counter, limg)

            rimg = self.cv_bridge.imgmsg_to_cv2(rmsg, "bgr8")
            cv2.imwrite("right_%02d.png" % self.save_counter, rimg)

            cloud_gen = pc2.read_points(vmsg)
            cloud = numpy.array(list(cloud_gen))
            # print(cloud.shape, cloud[0])
            numpy.savetxt(
                "cloud_%02d.pcd" % self.save_counter,
                cloud,
                fmt="%.7f %.7f %.7f %.7f %d %.7f",
                header=
                "VERSION 0.7\nFIELDS x y z intensity ring time\nSIZE SIZE 4 4 4 4 2 4\nTYPE F F F F U F\nWIDTH %d\nHEIGHT %d\nVIEWPOINT 0 0 0 1 0 0 0\nPOINTS %d\nDATA ascii"
                % (cloud.shape[0], cloud.shape[1],
                   cloud.shape[0] * cloud.shape[1]),
                comments="")

            self.save_counter += 1
Esempio n. 7
0
    def handle_stereo(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = StereoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    name=self._camera_name,
                    checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = StereoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    checkerboard_flags=self._checkerboard_flags)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
        self.redraw_stereo(drawable)
Esempio n. 8
0
    def handle_monocular(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = MonoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    name=self._camera_name,
                    checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = MonoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    checkerboard_flags=self.checkerboard_flags)

        # This should just call the MonoCalibrator
        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.scrib.shape[1]
        self.redraw_monocular(drawable)
Esempio n. 9
0
class CalibrationNode:
    def __init__(self,
                 boards,
                 service_check=True,
                 synchronizer=message_filters.TimeSynchronizer,
                 flags=0,
                 pattern=Patterns.Chessboard,
                 camera_name='',
                 checkerboard_flags=0):
        if service_check:
            # assume any non-default service names have been set.  Wait for the service to become ready
            for svcname in ["camera", "left_camera", "right_camera"]:
                remapped = rospy.remap_name(svcname)
                """
                if remapped != svcname:
                    fullservicename = "%s/set_camera_info" % remapped
                    print("Waiting for service", fullservicename, "...")
                    try:
                        rospy.wait_for_service(fullservicename, 5)
                        print("OK")
                    except rospy.ROSException:
                        print("Service not found")
                        rospy.signal_shutdown('Quit')
                """

        self._boards = boards
        self._calib_flags = flags
        self._checkerboard_flags = checkerboard_flags
        self._pattern = pattern
        self._camera_name = camera_name
        lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
        rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
        vsub = message_filters.Subscriber('laser', sensor_msgs.msg.PointCloud2)
        ts = synchronizer([lsub, rsub, vsub], 4)
        ts.registerCallback(self.queue_stereo)
        self.save_counter = 0
        self.cv_bridge = CvBridge()

        msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
        msub.registerCallback(self.queue_monocular)
        """
        self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"),
                                                          sensor_msgs.srv.SetCameraInfo)
        self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"),
                                                               sensor_msgs.srv.SetCameraInfo)
        self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"),
                                                                sensor_msgs.srv.SetCameraInfo)
        """
        self.q_mono = deque([], 1)
        self.q_stereo = deque([], 1)

        self.c = None

        mth = ConsumerThread(self.q_mono, self.handle_monocular)
        mth.setDaemon(True)
        mth.start()

        sth = ConsumerThread(self.q_stereo, self.handle_stereo)
        sth.setDaemon(True)
        sth.start()

    def redraw_stereo(self, *args):
        pass

    def redraw_monocular(self, *args):
        pass

    def queue_monocular(self, msg):
        self.q_mono.append(msg)

    def queue_stereo(self, lmsg, rmsg, vmsg):
        self.q_stereo.append((lmsg, rmsg, vmsg))

    def handle_monocular(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = MonoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    name=self._camera_name,
                    checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = MonoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    checkerboard_flags=self.checkerboard_flags)

        # This should just call the MonoCalibrator
        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.scrib.shape[1]
        self.redraw_monocular(drawable)

    def handle_stereo(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = StereoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    name=self._camera_name,
                    checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = StereoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    checkerboard_flags=self._checkerboard_flags)

        (lmsg, rmsg, vmsg) = msg
        (drawable, is_good_sample) = self.c.handle_msg((lmsg, rmsg))
        self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
        self.redraw_stereo(drawable)

        if is_good_sample:
            print "Saving good sample %d" % self.save_counter

            limg = self.cv_bridge.imgmsg_to_cv2(lmsg, "bgr8")
            cv2.imwrite("left_%02d.png" % self.save_counter, limg)

            rimg = self.cv_bridge.imgmsg_to_cv2(rmsg, "bgr8")
            cv2.imwrite("right_%02d.png" % self.save_counter, rimg)

            cloud_gen = pc2.read_points(vmsg)
            cloud = numpy.array(list(cloud_gen))
            # print(cloud.shape, cloud[0])
            numpy.savetxt(
                "cloud_%02d.pcd" % self.save_counter,
                cloud,
                fmt="%.7f %.7f %.7f %.7f %d %.7f",
                header=
                "VERSION 0.7\nFIELDS x y z intensity ring time\nSIZE SIZE 4 4 4 4 2 4\nTYPE F F F F U F\nWIDTH %d\nHEIGHT %d\nVIEWPOINT 0 0 0 1 0 0 0\nPOINTS %d\nDATA ascii"
                % (cloud.shape[0], cloud.shape[1],
                   cloud.shape[0] * cloud.shape[1]),
                comments="")

            self.save_counter += 1

    def check_set_camera_info(self, response):
        if response.success:
            return True

        for i in range(10):
            print("!" * 80)
        print()
        print("Attempt to set camera info failed: " + response.status_message)
        print()
        for i in range(10):
            print("!" * 80)
        print()
        rospy.logerr(
            'Unable to set camera info for calibration. Failure message: %s' %
            response.status_message)
        return False

    def do_upload(self):
        self.c.report()
        print(self.c.ost())
        info = self.c.as_message()

        rv = True
        """
        if self.c.is_mono:
            response = self.set_camera_info_service(info)
            rv = self.check_set_camera_info(response)
        else:
            response = self.set_left_camera_info_service(info[0])
            rv = rv and self.check_set_camera_info(response)
            response = self.set_right_camera_info_service(info[1])
            rv = rv and self.check_set_camera_info(response)
        """
        return rv
Esempio n. 10
0
class CalibrationNode:
    def __init__(self,
                 boards,
                 camera_namespace,
                 image_topic,
                 synchronizer=message_filters.TimeSynchronizer,
                 pattern=Patterns.Chessboard,
                 directory='/tmp',
                 min_samples=0):

        fullservicename = "%s/set_camera_info" % camera_namespace
        print "Waiting for service %s ..." % (fullservicename)
        try:
            rospy.wait_for_service(fullservicename, 5)
            print("OK")
        except rospy.ROSException:
            print("Service not found")
            rospy.signal_shutdown('Quit')

        #if service_check:
        ## assume any non-default service names have been set.  Wait for the service to become ready
        #for svcname in ["camera", "left_camera", "right_camera"]:
        #remapped = rospy.remap_name(svcname)
        #if remapped != svcname:
        #fullservicename = "%s/set_camera_info" % remapped
        #print "Waiting for service %s ..." %(fullservicename)
        #try:
        #rospy.wait_for_service(fullservicename, 5)
        #print("OK")
        #except rospy.ROSException:
        #print("Service not found")
        #rospy.signal_shutdown('Quit')

        # Minimum number of samples to capture
        if min_samples <= 0:
            self.min_samples_number = float('inf')
        else:
            self.min_samples_number = min_samples
        self._boards = boards
        self._calib_flags = 0
        self._pattern = pattern
        self._camera_name = ''  # Delete?
        self._directory = directory
        lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
        rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
        ts = synchronizer([lsub, rsub], 4)
        ts.registerCallback(self.queue_stereo)

        msub = message_filters.Subscriber(image_topic, sensor_msgs.msg.Image)
        msub.registerCallback(self.queue_monocular)

        self.set_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % camera_namespace,
            sensor_msgs.srv.SetCameraInfo)
        self.set_left_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % rospy.remap_name("left_camera"),
            sensor_msgs.srv.SetCameraInfo)
        self.set_right_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % rospy.remap_name("right_camera"),
            sensor_msgs.srv.SetCameraInfo)

        self.q_mono = Queue()
        self.q_stereo = Queue()

        self.c = None
        self.captured_images_count = 0
        self.capture_img_permission = False
        self.image_captured = False
        self.image_captured_server()

        mth = ConsumerThread(self.q_mono, self.handle_monocular)
        mth.setDaemon(True)
        mth.start()

        sth = ConsumerThread(self.q_stereo, self.handle_stereo)
        sth.setDaemon(True)
        sth.start()

        self.enough_samples = False
        self.check_goodenough()

        rospy.signal_shutdown('Quit')

    def redraw_stereo(self, *args):
        pass

    def redraw_monocular(self, *args):
        pass

    def queue_monocular(self, msg):
        self.q_mono.put(msg)

    def queue_stereo(self, lmsg, rmsg):
        self.q_stereo.put((lmsg, rmsg))

    def handle_monocular(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = MonoCalibrator(self._boards,
                                        self._calib_flags,
                                        self._pattern,
                                        name=self._camera_name)
            else:
                self.c = MonoCalibrator(self._boards, self._calib_flags,
                                        self._pattern)
        if self.min_samples_number >= self.captured_images_count and self.c.goodenough:
            self.enough_samples = True
        if not self.capture_img_permission and not self.enough_samples:
            self.permission_query()
        if self.capture_img_permission and not self.image_captured:
            # This should just call the MonoCalibrator
            drawable = self.c.handle_msg(msg)
            self.displaywidth = drawable.scrib.cols
            #self.redraw_monocular(drawable)
            if len(self.c.db) > self.captured_images_count:
                print 'len(self.c.db): %s    pic_count: %s' % (len(
                    self.c.db), self.captured_images_count)
                print 'Image captured'
                self.captured_images_count += 1
                self.capture_img_permission = False
                self.image_captured = True

    def check_goodenough(self):
        while not self.enough_samples:
            pass
        raw_input(
            'Enough samples captured. Press Enter to calculate the calibration parameters...'
        )
        self.c.do_calibration()
        self.c.do_save(self._directory)
        print 'Calibration finished.'
        raw_input(
            'Press Enter to upload the new camera parameters to the camera driver...'
        )
        self.do_upload()

    def permission_query(self):
        rospy.sleep(0.5)
        try:
            rospy.wait_for_service('capture_img_permission', 5)
        except:
            print("Service not found")
            rospy.signal_shutdown('Quit')
        query = rospy.ServiceProxy('capture_img_permission', PermissionTrig)
        print 'Waiting for a permission to capture sample image...'
        try:
            resp = query()
            if resp.capture_img == True:
                print 'Permission given'
                self.capture_img_permission = True
        except rospy.ServiceException as exc:
            print str(exc)
            rospy.signal_shutdown('Quit')
        self.image_captured = False

    def handle_stereo(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = StereoCalibrator(self._boards,
                                          self._calib_flags,
                                          self._pattern,
                                          name=self._camera_name)
            else:
                self.c = StereoCalibrator(self._boards, self._calib_flags,
                                          self._pattern)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.cols + drawable.rscrib.cols
        self.redraw_stereo(drawable)

    def check_set_camera_info(self, response):
        if response.success:
            return True

        for i in range(10):
            print("!" * 80)
        print()
        print("Attempt to set camera info failed: " + response.status_message)
        print()
        for i in range(10):
            print("!" * 80)
        print()
        rospy.logerr(
            'Unable to set camera info for calibration. Failure message: %s' %
            response.status_message)
        return False

    def do_upload(self):
        self.c.report()
        print(self.c.ost())
        info = self.c.as_message()

        rv = True
        if self.c.is_mono:
            response = self.set_camera_info_service(info)
            rv = self.check_set_camera_info(response)
        else:
            response = self.set_left_camera_info_service(info[0])
            rv = rv and self.check_set_camera_info(response)
            response = self.set_right_camera_info_service(info[1])
            rv = rv and self.check_set_camera_info(response)
        return rv

    def handle_image_captured(self, req):
        return self.image_captured

    def image_captured_server(self):
        s = rospy.Service('image_captured', PermissionTrig,
                          self.handle_image_captured)
        print "image_captured_server up"
Esempio n. 11
0
class CalibrationNode:
    def __init__(self,
                 boards,
                 service_check=True,
                 synchronizer=message_filters.TimeSynchronizer,
                 flags=0,
                 pattern=Patterns.Chessboard,
                 camera_name='',
                 checkerboard_flags=0):
        if service_check:
            # assume any non-default service names have been set.  Wait for the service to become ready
            for svcname in ["camera", "left_camera", "right_camera"]:
                remapped = rospy.remap_name(svcname)
                if remapped != svcname:
                    fullservicename = "%s/set_camera_info" % remapped
                    print("Waiting for service", fullservicename, "...")
                    try:
                        rospy.wait_for_service(fullservicename, 5)
                        print("OK")
                    except rospy.ROSException:
                        print("Service not found")
                        rospy.signal_shutdown('Quit')

        self._boards = boards
        self._calib_flags = flags
        self._checkerboard_flags = checkerboard_flags
        self._pattern = pattern
        self._camera_name = camera_name
        lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
        rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
        ts = synchronizer([lsub, rsub], 4)
        ts.registerCallback(self.queue_stereo)

        msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
        msub.registerCallback(self.queue_monocular)

        self.set_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % rospy.remap_name("camera"),
            sensor_msgs.srv.SetCameraInfo)
        self.set_left_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % rospy.remap_name("left_camera"),
            sensor_msgs.srv.SetCameraInfo)
        self.set_right_camera_info_service = rospy.ServiceProxy(
            "%s/set_camera_info" % rospy.remap_name("right_camera"),
            sensor_msgs.srv.SetCameraInfo)

        self.q_mono = Queue()
        self.q_stereo = Queue()

        self.c = None

        mth = ConsumerThread(self.q_mono, self.handle_monocular)
        mth.setDaemon(True)
        mth.start()

        sth = ConsumerThread(self.q_stereo, self.handle_stereo)
        sth.setDaemon(True)
        sth.start()

    def redraw_stereo(self, *args):
        pass

    def redraw_monocular(self, *args):
        pass

    def queue_monocular(self, msg):
        try:
            self.q_mono.get(False, 0)
        except QueueEmptyException:
            pass
        self.q_mono.put(msg)

    def queue_stereo(self, lmsg, rmsg):
        try:
            self.q_stereo.get(False, 0)
        except QueueEmptyException:
            pass
        self.q_stereo.put((lmsg, rmsg))

    def handle_monocular(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = MonoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    name=self._camera_name,
                    checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = MonoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    checkerboard_flags=self.checkerboard_flags)

        # This should just call the MonoCalibrator
        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.scrib.shape[1]
        self.redraw_monocular(drawable)

    def handle_stereo(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = StereoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    name=self._camera_name,
                    checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = StereoCalibrator(
                    self._boards,
                    self._calib_flags,
                    self._pattern,
                    checkerboard_flags=self._checkerboard_flags)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
        self.redraw_stereo(drawable)

    def check_set_camera_info(self, response):
        if response.success:
            return True

        for i in range(10):
            print("!" * 80)
        print()
        print("Attempt to set camera info failed: " + response.status_message)
        print()
        for i in range(10):
            print("!" * 80)
        print()
        rospy.logerr(
            'Unable to set camera info for calibration. Failure message: %s' %
            response.status_message)
        return False

    def do_upload(self):
        self.c.report()
        print(self.c.ost())
        info = self.c.as_message()

        rv = True
        if self.c.is_mono:
            response = self.set_camera_info_service(info)
            rv = self.check_set_camera_info(response)
        else:
            response = self.set_left_camera_info_service(info[0])
            rv = rv and self.check_set_camera_info(response)
            response = self.set_right_camera_info_service(info[1])
            rv = rv and self.check_set_camera_info(response)
        return rv
Esempio n. 12
0
class CalibrationNode:
    def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
                 pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0):
        if service_check:
            # assume any non-default service names have been set.  Wait for the service to become ready
            for svcname in ["camera", "left_camera", "right_camera"]:
                remapped = rospy.remap_name(svcname)
                if remapped != svcname:
                    fullservicename = "%s/set_camera_info" % remapped
                    print("Waiting for service", fullservicename, "...")
                    try:
                        rospy.wait_for_service(fullservicename, 5)
                        print("OK")
                    except rospy.ROSException:
                        print("Service not found")
                        rospy.signal_shutdown('Quit')

        self._boards = boards
        self._calib_flags = flags
        self._checkerboard_flags = checkerboard_flags
        self._pattern = pattern
        self._camera_name = camera_name
        lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
        rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
        ts = synchronizer([lsub, rsub], 4)
        ts.registerCallback(self.queue_stereo)

        msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
        msub.registerCallback(self.queue_monocular)

        calibrateService = rospy.Service('calibrates', Calibrate, self.do_calibrate)
        
        self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"),
                                                          sensor_msgs.srv.SetCameraInfo)
        self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"),
                                                               sensor_msgs.srv.SetCameraInfo)
        self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"),
                                                                sensor_msgs.srv.SetCameraInfo)

        self.q_mono = deque([], 1)
        self.q_stereo = deque([], 1)

        self.c = None

        self.mth = ConsumerThread(self.q_mono, self.handle_monocular)
        self.mth.setDaemon(True)
        self.mth.start()

        sth = ConsumerThread(self.q_stereo, self.handle_stereo)
        sth.setDaemon(True)
        sth.start()
        
    def do_calibrate(self,req):
        print "do_calibrate"
        if req.ready:
            print "ready"
            self.c.do_calibration()
            if self.c.calibrated:
#            if True:
                print "calibrated"
                #self.c.do_save();
                self.c.save_txt();
                return CalibrateResponse(True)
   
    def redraw_stereo(self, *args):
        pass
    def redraw_monocular(self, *args):
        pass

    def queue_monocular(self, msg):
        self.q_mono.append(msg)

    def queue_stereo(self, lmsg, rmsg):
        self.q_stereo.append((lmsg, rmsg))

    def handle_monocular(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
                                        checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern,
                                        checkerboard_flags=self.checkerboard_flags)

        # This should just call the MonoCalibrator
        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.scrib.shape[1]
        self.redraw_monocular(drawable)

#        print "drawable_scrib"
          
        pub = rospy.Publisher('goodenoughTopic', Bool, queue_size=10)
        hello_str = self.c.goodenough
        rospy.loginfo(hello_str)
        pub.publish(hello_str)

        bridge = cv_bridge.CvBridge()
        pub1 = rospy.Publisher('boardImage', sensor_msgs.msg.Image, queue_size=10)
        pub1.publish(bridge.cv2_to_imgmsg(drawable.scrib, "bgr8"))

        #calibrate picture enough, must stop thread
        if self.c.goodenough:
            self.mth.stop()
        
    def handle_stereo(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
                                          checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern,
                                          checkerboard_flags=self._checkerboard_flags)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
        self.redraw_stereo(drawable)
            
 
    def check_set_camera_info(self, response):
        if response.success:
            return True

        for i in range(10):
            print("!" * 80)
        print()
        print("Attempt to set camera info failed: " + response.status_message)
        print()
        for i in range(10):
            print("!" * 80)
        print()
        rospy.logerr('Unable to set camera info for calibration. Failure message: %s' % response.status_message)
        return False

    def do_upload(self):
        self.c.report()
        print(self.c.ost())
        info = self.c.as_message()

        rv = True
        if self.c.is_mono:
            response = self.set_camera_info_service(info)
            rv = self.check_set_camera_info(response)
        else:
            response = self.set_left_camera_info_service(info[0])
            rv = rv and self.check_set_camera_info(response)
            response = self.set_right_camera_info_service(info[1])
            rv = rv and self.check_set_camera_info(response)
        return rv
class CalibrationNode:
    def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
                 pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0):
        if service_check:
            # assume any non-default service names have been set.  Wait for the service to become ready
            for svcname in ["camera", "left_camera", "right_camera"]:
                remapped = rospy.remap_name(svcname)
                if remapped != svcname:
                    fullservicename = "%s/set_camera_info" % remapped
                    print("Waiting for service", fullservicename, "...")
                    try:
                        rospy.wait_for_service(fullservicename, 5)
                        print("OK")
                    except rospy.ROSException:
                        print("Service not found")
                        rospy.signal_shutdown('Quit')

        self._boards = boards
        self._calib_flags = flags
        self._checkerboard_flags = checkerboard_flags
        self._pattern = pattern
        self._camera_name = camera_name
        lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image)
        rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image)
        ts = synchronizer([lsub, rsub], 4)
        ts.registerCallback(self.queue_stereo)

        msub = message_filters.Subscriber('image', sensor_msgs.msg.Image)
        msub.registerCallback(self.queue_monocular)
        
        self.set_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("camera"),
                                                          sensor_msgs.srv.SetCameraInfo)
        self.set_left_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("left_camera"),
                                                               sensor_msgs.srv.SetCameraInfo)
        self.set_right_camera_info_service = rospy.ServiceProxy("%s/set_camera_info" % rospy.remap_name("right_camera"),
                                                                sensor_msgs.srv.SetCameraInfo)

        self.q_mono = Queue()
        self.q_stereo = Queue()

        self.c = None

        mth = ConsumerThread(self.q_mono, self.handle_monocular)
        mth.setDaemon(True)
        mth.start()

        sth = ConsumerThread(self.q_stereo, self.handle_stereo)
        sth.setDaemon(True)
        sth.start()
        
    def redraw_stereo(self, *args):
        pass
    def redraw_monocular(self, *args):
        pass

    def queue_monocular(self, msg):
        try:
            self.q_mono.get(False, 0)
        except QueueEmptyException:
            pass
        self.q_mono.put(msg)

    def queue_stereo(self, lmsg, rmsg):
        try:
            self.q_stereo.get(False, 0)
        except QueueEmptyException:
            pass
        self.q_stereo.put((lmsg, rmsg))

    def handle_monocular(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
                                        checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern,
                                        checkerboard_flags=self.checkerboard_flags)

        # This should just call the MonoCalibrator
        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.scrib.shape[1]
        self.redraw_monocular(drawable)

    def handle_stereo(self, msg):
        if self.c == None:
            if self._camera_name:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name,
                                          checkerboard_flags=self._checkerboard_flags)
            else:
                self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern,
                                          checkerboard_flags=self._checkerboard_flags)

        drawable = self.c.handle_msg(msg)
        self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
        self.redraw_stereo(drawable)
            
 
    def check_set_camera_info(self, response):
        if response.success:
            return True

        for i in range(10):
            print("!" * 80)
        print()
        print("Attempt to set camera info failed: " + response.status_message)
        print()
        for i in range(10):
            print("!" * 80)
        print()
        rospy.logerr('Unable to set camera info for calibration. Failure message: %s' % response.status_message)
        return False

    def do_upload(self):
        self.c.report()
        print(self.c.ost())
        info = self.c.as_message()

        rv = True
        if self.c.is_mono:
            response = self.set_camera_info_service(info)
            rv = self.check_set_camera_info(response)
        else:
            response = self.set_left_camera_info_service(info[0])
            rv = rv and self.check_set_camera_info(response)
            response = self.set_right_camera_info_service(info[1])
            rv = rv and self.check_set_camera_info(response)
        return rv