class CalibrationNode:
def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
fisheye_flags = 0, pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0,
max_chessboard_speed = -1, queue_size = 1):
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._fisheye_calib_flags = fisheye_flags
self._checkerboard_flags = checkerboard_flags
self._pattern = pattern
self._camera_name = camera_name
self._max_chessboard_speed = max_chessboard_speed
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 = BufferQueue(queue_size)
self.q_stereo = BufferQueue(queue_size)
self.c = None
self._last_display = 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.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._fisheye_calib_flags, self._pattern, name=self._camera_name,
checkerboard_flags=self._checkerboard_flags,
max_chessboard_speed = self._max_chessboard_speed)
else:
self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern,
checkerboard_flags=self.checkerboard_flags,
max_chessboard_speed = self._max_chessboard_speed)
# 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._fisheye_calib_flags, self._pattern, name=self._camera_name,
checkerboard_flags=self._checkerboard_flags,
max_chessboard_speed = self._max_chessboard_speed)
else:
self.c = StereoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern,
checkerboard_flags=self._checkerboard_flags,
max_chessboard_speed = self._max_chessboard_speed)
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 = 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):
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)
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(Node):
def __init__(self,
boards,
service_check=False,
synchronizer=message_filters.TimeSynchronizer,
flags=0,
pattern=Patterns.Chessboard,
camera_name='camera',
camera='camera',
left_camera='left_camera',
right_camera='right_camera',
image='image',
checkerboard_flags=0):
# TODO will enable this function as soon as set_camera_info enabled
# 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 = svcname
# if remapped != svcname:
# fullservicename = "%s/set_camera_info" % remapped
# print("Waiting for service", fullservicename, "...")
# try:
# rclpy.wait_for_service(fullservicename, 5)
# print("OK")
# except:
# print("Service not found")
# rclpy.shutdown()
super().__init__(self._node_name)
self._boards = boards
self._calib_flags = flags
self._checkerboard_flags = checkerboard_flags
self._pattern = pattern
self._camera_name = camera_name
lsub = message_filters.Subscriber(self, sensor_msgs.msg.Image,
left_camera)
rsub = message_filters.Subscriber(self, sensor_msgs.msg.Image,
right_camera)
ts = synchronizer([lsub, rsub], 4)
ts.registerCallback(self.queue_stereo)
msub = message_filters.Subscriber(self, sensor_msgs.msg.Image, image)
msub.registerCallback(self.queue_monocular)
self.set_camera_info_cli = self.create_client(
sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % camera)
self.set_left_camera_cli = self.create_client(
sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % left_camera)
self.set_right_camera_cli = self.create_client(
sensor_msgs.srv.SetCameraInfo, "%s/set_camera_info" % right_camera)
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):
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)
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.result().success:
return True
for i in range(10):
print("!" * 80)
print()
print("Attempt to set camera info failed: " +
response.result().status_message)
print()
for i in range(10):
print("!" * 80)
print()
self.get_logger().error(
'Unable to set camera info for calibration. Failure message: %s' %
response.result().status_message)
return False
def do_upload(self):
self.c.report()
print(self.c.ost())
info = self.c.as_message()
req = sensor_msgs.srv.SetCameraInfo.Request()
rv = True
if self.c.is_mono:
req.camera_info = info
response = self.set_camera_info_cli.call_async(req)
rv = self.check_set_camera_info(response)
else:
req.camera_info = info[0]
response = self.set_left_camera_info_service(req)
rv = rv and self.check_set_camera_info(response)
req.camera_info = info[1]
response = self.set_right_camera_info_service(req)
rv = rv and self.check_set_camera_info(response)
return rv
class CalibrationNode(Node):
def __init__(self,
name,
boards,
service_check=True,
synchronizer=message_filters.TimeSynchronizer,
flags=0,
pattern=Patterns.Chessboard,
camera_name='',
checkerboard_flags=0):
super().__init__(name)
self.set_camera_info_service = self.create_client(
sensor_msgs.srv.SetCameraInfo, "camera/set_camera_info")
self.set_left_camera_info_service = self.create_client(
sensor_msgs.srv.SetCameraInfo, "left_camera/set_camera_info")
self.set_right_camera_info_service = self.create_client(
sensor_msgs.srv.SetCameraInfo, "right_camera/set_camera_info")
if service_check:
# assume any non-default service names have been set. Wait for the service to become ready
for cli in [
self.set_camera_info_service,
self.set_left_camera_info_service,
self.set_right_camera_info_service
]:
#remapped = rclpy.remap_name(svcname)
#if remapped != svcname:
#fullservicename = "%s/set_camera_info" % remapped
print("Waiting for service", cli.srv_name, "...")
# check all services so they are ready.
try:
cli.wait_for_service(timeout_sec=5)
print("OK")
except Exception as e:
print("Service not found: %s".format(e))
rclpy.shutdown()
self._boards = boards
self._calib_flags = flags
self._checkerboard_flags = checkerboard_flags
self._pattern = pattern
self._camera_name = camera_name
lsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'left')
rsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'right')
ts = synchronizer([lsub, rsub], 4)
ts.registerCallback(self.queue_stereo)
msub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'image')
msub.registerCallback(self.queue_monocular)
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):
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)
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.done():
if response.result() is not None:
if response.result().success:
return True
for i in range(10):
print("!" * 80)
print()
print("Attempt to set camera info failed: " +
response.result() if response.result(
) is not None else "Not available")
print()
for i in range(10):
print("!" * 80)
print()
self.get_logger().error(
'Unable to set camera info for calibration. Failure message: %s' %
response.result() if response.result(
) is not None else "Not available")
return False
def do_upload(self):
self.c.report()
print(self.c.ost())
info = self.c.as_message()
req = sensor_msgs.srv.SetCameraInfo.Request()
rv = True
if self.c.is_mono:
req.camera_info = info
response = self.set_camera_info_service.call_async(req)
rv = self.check_set_camera_info(response)
else:
req.camera_info = info[0]
response = self.set_left_camera_info_service.call_async(req)
rv = rv and self.check_set_camera_info(response)
req.camera_info = info[1]
response = self.set_right_camera_info_service.call_async(req)
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 = deque([], 1)
self.q_stereo = deque([], 1)
self.c = None
# add publisher when received the corner information TODO:
self.left_pub = rospy.Publisher('/left_corners',
Float32MultiArray,
queue_size=100)
self.right_pub = rospy.Publisher('/right_corners',
Float32MultiArray,
queue_size=100)
self.lcorner_size_pub = rospy.Publisher('/get_corner_size_l',
Int32,
queue_size=100)
self.rcorner_size_pub = rospy.Publisher('/get_corner_size_r',
Int32,
queue_size=100)
# self.intrinsic_pub = rospy.Publisher('/camera_intrinsics', intrinsic_param, queue_size = 100)
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):
self.q_stereo.append((lmsg, rmsg))
def convert_point2f_to_list(
self, input_corners
): # change cv::Point2f(numpy.ndarray in python) to list
corner_Xs = input_corners[:, :, 0]
corner_Ys = input_corners[:, :, 1]
new_corner = zip(corner_Xs, corner_Ys)
corner_tuple = tuple(new_corner)
return corner_tuple
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)
# added lines TODO:
# left_temp = points() #get msg type from corners
# right_temp = points() #get msg type from corners
# corner_msgs = corners()
lcorner_size_msg = Int32()
rcorner_size_msg = Int32()
# The mat is giving the ready-to-publish corner coordinates, push everything in a mat TODO:
if drawable.lcorner is not None:
lcorner_size = len(drawable.lcorner)
# publish the coner size for computation
lcorner_size_msg.data = lcorner_size
self.lcorner_size_pub.publish(lcorner_size_msg)
left_mat = Float32MultiArray()
left_mat.layout.dim.append(MultiArrayDimension())
left_mat.layout.dim.append(MultiArrayDimension())
left_mat.layout.dim[0].label = "row"
left_mat.layout.dim[1].label = "col"
left_mat.layout.dim[
0].size = lcorner_size # if there are 9 points give 9 dimension
left_mat.layout.dim[1].size = lcorner_size
left_mat.layout.dim[0].stride = lcorner_size
left_mat.layout.dim[1].stride = lcorner_size
left_mat.layout.data_offset = 0
left_mat.data = [0] * lcorner_size * lcorner_size
dstride1 = left_mat.layout.dim[1].stride
offset = left_mat.layout.data_offset
print("left") # debug
print(drawable.lcorner) # debug
i_l = 0
for temp_left in drawable.lcorner:
temp_x = temp_left[0, 0]
temp_y = temp_left[0, 1]
left_mat.data[offset + i_l + dstride1 * 0] = temp_x
left_mat.data[offset + i_l + dstride1 * 1] = temp_y
i_l += 1
self.left_pub.publish(left_mat)
else:
print()
print("No LEFT corner coordinates")
print()
if drawable.rcorner is not None:
rcorner_size = len(drawable.rcorner)
rcorner_size_msg.data = rcorner_size
self.rcorner_size_pub.publish(rcorner_size_msg)
right_mat = Float32MultiArray()
right_mat.layout.dim.append(MultiArrayDimension())
right_mat.layout.dim.append(MultiArrayDimension())
right_mat.layout.dim[0].label = "row"
right_mat.layout.dim[1].label = "col"
right_mat.layout.dim[0].size = rcorner_size
right_mat.layout.dim[1].size = rcorner_size
right_mat.layout.dim[0].stride = rcorner_size
right_mat.layout.dim[1].stride = rcorner_size
right_mat.layout.data_offset = 0
right_mat.data = [0] * rcorner_size * rcorner_size
dstride1 = right_mat.layout.dim[1].stride
offset = right_mat.layout.data_offset
print("right") # debug
print(drawable.rcorner) #debug
i_r = 0
for temp_right in drawable.rcorner:
temp_x = temp_right[0, 0]
temp_y = temp_right[0, 1]
right_mat.data[offset + i_r + dstride1 * 0] = temp_x
right_mat.data[offset + i_r + dstride1 * 1] = temp_y
i_r += 1
self.right_pub.publish(right_mat)
else:
print()
print("No RIGHT corner coordinates")
print()
# tryp to publisht the intrinsic matrix for left and right camera
# if self.c.is_mono:
# pass
# else:
# stereo_cam_info = self.c.as_message()
# left_response = self.set_left_camera_info_service(stereo_cam_info[0])
# left_intrinsic = left_response.camera_info.K
# right_response = self.set_right_camera_info_service(stereo_cam_info[1])
# right_intrinsic = right_response.camera_info.K
# camera_intrinsics_msg.left_intirnsic = left_intrinsic
# camera_intrinsics_msg.right_intirnsic = right_intrinsic
# self.intrinsic_pub.publish(camera_intrinsics_msg)
rospy.sleep(0.1)
# end of the handle_strero
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='',
detection='cv2',
output='yaml',
checkerboard_flags=0,
min_good_enough=40):
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._detection = detection
self._output = output
self._calib_flags = flags
self._checkerboard_flags = checkerboard_flags
self._pattern = pattern
self._camera_name = camera_name
self._min_good_enough = min_good_enough
rospack = rospkg.RosPack()
pkg_path = rospack.get_path('calibration')
self._autoware_image = cv2.imread(
path.join(pkg_path, 'docs/smartcar_logo.png'),
cv2.IMREAD_UNCHANGED)
self._autoware_image = cv2.resize(self._autoware_image, (100, 25))
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 = 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):
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,
detection=self._detection,
output=self._output,
checkerboard_flags=self._checkerboard_flags,
min_good_enough=self._min_good_enough)
else:
self.c = MonoCalibrator(
self._boards,
self._calib_flags,
self._pattern,
detection=self._detection,
output=self._output,
checkerboard_flags=self.checkerboard_flags,
min_good_enough=self._min_good_enough)
# 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,
detection=self._detection,
output=self._output,
checkerboard_flags=self._checkerboard_flags,
min_good_enough=self._min_good_enough)
else:
self.c = StereoCalibrator(
self._boards,
self._calib_flags,
self._pattern,
detection=self._detection,
output=self._output,
checkerboard_flags=self.checkerboard_flags,
min_good_enough=self._min_good_enough)
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