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 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 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)
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 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 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)
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
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"
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
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