def increment_led(self):
"""
Increments the led array indices. When the last led is reached check to see if
sufficient points have been captured for the homography calibratoin. If so, then
the state is changed to FINISHED. If insufficient points have been gathered then
the state is changed back to WAITING.
"""
if self.led_m < self.led_m_max - 1 or self.led_n < self.led_n_max - 1:
if self.led_n < self.led_n_max - 1:
self.led_n += 1
else:
self.led_n = 0
self.led_m += 1
else:
# Turn off led and unlock active target
mct_active_target.off()
mct_active_target.unlock(self.node_name)
# Need to check that we got enough done otherwise return to idle.
if len(self.image_points) >= self.num_points_required:
self.state = FINISHED
self.image_info = ''
else:
self.state = WAITING
self.image_info = 'not enough data'
self.homography_matrix = None
def kill_nodes():
if not DEVELOP:
print(' * killing nodes ... ',end='')
mct_active_target.off()
mjpeg_servers.stop_servers()
zoom_tool_master.stop()
camera_master.stop_cameras()
print('done')
def __init__(self, topic):
self.ready = False
self.state = WAITING # There are 3 allowed states WAITING, WORKING, FINISHED
self.topic = topic
self.bridge = CvBridge()
self.lock = threading.Lock()
rospy.init_node('homography_calibrator')
self.node_name = rospy.get_name()
# Initialize data lists
self.blobs_list = []
self.image_points = []
self.world_points = []
# Set active target information and turn off leds
target_info = mct_active_target.active_target_info()
self.led_n_max = target_info[0]
self.led_m_max = target_info[1]
self.number_of_leds = self.led_n_max * self.led_m_max
self.led_max_power = target_info[2]
self.led_space_x = target_info[3]
self.led_space_y = target_info[3]
mct_active_target.off()
# Current led indices
self.led_n = 0
self.led_m = 0
# Led power setting
params_namespace = '/homography_calibrator_params'
self.led_power = rospy.get_param(
'{0}/target/led_power'.format(params_namespace), 10)
# Wait count for image acquisition
self.image_wait_number = rospy.get_param(
'{0}/image_wait_number'.format(params_namespace), 4)
self.image_wait_cnt = 0
# Sleep periods for idle and wait count loops
self.idle_sleep_dt = 0.25
self.wait_sleep_dt = 0.005
# Initialize blob finder
self.blobFinder = BlobFinder()
self.blobFinder.threshold = rospy.get_param(
'{0}/blob_finder/threshold'.format(params_namespace), 200)
self.blobFinder.filter_by_area = rospy.get_param(
'{0}/blob_finder/filter_by_area'.format(params_namespace), False)
self.blobFinder.min_area = rospy.get_param(
'{0}/blob_finder/min_area'.format(params_namespace), 0)
self.blobFinder.max_area = rospy.get_param(
'{0}/blob_finder/max_area'.format(params_namespace), 200)
# Initialize homography matrix and number of points required to solve for it
self.homography_matrix = None
self.num_points_required = rospy.get_param(
'{0}/num_points_required'.format(params_namespace), 10)
# Set font and initial image information
self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX,
0.8,
0.8,
thickness=1)
self.image_info = 'no data'
# Subscription to image topic
self.image_sub = rospy.Subscriber(self.topic, Image,
self.image_callback)
# Publications - blobs image and calibration progress image
self.image_blobs_pub = rospy.Publisher('image_homography_blobs', Image)
self.image_calib_pub = rospy.Publisher('image_homography_calibration',
Image)
# Services
self.start_srv = rospy.Service('{0}/start'.format(self.node_name),
GetFlagAndMessage,
self.handle_start_srv)
self.get_matrix_srv = rospy.Service(
'{0}/get_matrix'.format(self.node_name), GetMatrix,
self.handle_get_matrix_srv)
self.is_calibrated_srv = rospy.Service(
'{0}/is_calibrated'.format(self.node_name), GetBool,
self.handle_is_calibrated_srv)
self.ready = True
def __init__(self, topic_0, topic_1):
self.ready = False
self.state = WAITING # There are 3 allowed states WAITING, WORKING, FINISHED
#self.state = WORKING
self.topics = topic_0, topic_1
self.bridge = CvBridge()
self.lock = threading.Lock()
rospy.init_node('transform_2d_calibrator')
self.node_name = rospy.get_name()
# Initialize data lists
self.blobs = {
self.topics[0]: [],
self.topics[1]: [],
}
self.index_to_image = {
self.topics[0]: {},
self.topics[1]: {},
}
self.overlap_indices = []
# Set active target information and turn off leds
target_info = mct_active_target.active_target_info()
self.led_n_max = target_info[0]
self.led_m_max = target_info[1]
self.number_of_leds = self.led_n_max * self.led_m_max
self.led_max_power = target_info[2]
self.led_space_x = target_info[3]
self.led_space_y = target_info[3]
mct_active_target.off()
# Current led indices
self.led_n = 0
self.led_m = 0
# Wait counter for image acquisition
self.image_wait_cnt = {
self.topics[0]: 0,
self.topics[1]: 0,
}
# Sleep periods for idle and wait count loops
self.idle_sleep_dt = 0.25
self.wait_sleep_dt = 0.005
# Led power setting
params_namespace = '/transform_2d_calibrator_params'
self.led_power = rospy.get_param(
'{0}/target/led_power'.format(params_namespace), 10)
# Wait count for image acquisition
self.image_wait_number = rospy.get_param(
'{0}/image_wait_number'.format(params_namespace), 4)
# Initialize blob finder
self.blobFinder = BlobFinder()
self.blobFinder.threshold = rospy.get_param(
'{0}/blob_finder/threshold'.format(params_namespace), 150)
self.blobFinder.filter_by_area = rospy.get_param(
'{0}/blob_finder/filter_by_area'.format(params_namespace), False)
self.blobFinder.min_area = rospy.get_param(
'{0}/blob_finder/min_area'.format(params_namespace), 0)
self.blobFinder.max_area = rospy.get_param(
'{0}/blob_finder/max_area'.format(params_namespace), 200)
# Number of points required
self.num_points_required = rospy.get_param(
'{0}/num_points_required'.format(params_namespace), 10)
self.transform = None
self.transform_error = None
# Get homography matrices
self.homography_dict = {}
for topic in self.topics:
try:
# Get the data from the parameter server
rows = rospy.get_param(
'{0}/homography_matrix/rows'.format(topic))
cols = rospy.get_param(
'{0}/homography_matrix/cols'.format(topic))
data = rospy.get_param(
'{0}/homography_matrix/data'.format(topic))
except KeyError:
# Data is not on the parameter server - try getting it by reading the file
camera = get_camera_from_topic(topic)
homography = file_tools.read_homography_calibration(camera)
rows = homography['rows']
cols = homography['cols']
data = homography['data']
# Rearrange into numpy matrix
homography_matrix = numpy.array(homography['data'])
homography_matrix = homography_matrix.reshape((rows, cols))
self.homography_dict[topic] = homography_matrix
# Set font and initial image information
self.cv_text_font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX,
0.8,
0.8,
thickness=1)
self.image_info = 'no data'
# Subscription to image topic
image_callback_0 = functools.partial(self.image_callback,
self.topics[0])
image_callback_1 = functools.partial(self.image_callback,
self.topics[1])
self.image_sub = {
self.topics[0]:
rospy.Subscriber(self.topics[0], Image, image_callback_0),
self.topics[1]:
rospy.Subscriber(self.topics[1], Image, image_callback_1),
}
# Publications - bcalibration progress images for topics 0 and 1
self.image_pub = {
self.topics[0]:
rospy.Publisher('image_transform_calibration_0', Image),
self.topics[1]:
rospy.Publisher('image_transform_calibration_1', Image),
}
# Services
self.start_srv = rospy.Service('{0}/start'.format(self.node_name),
GetFlagAndMessage,
self.handle_start_srv)
self.is_calibrated_srv = rospy.Service(
'{0}/is_calibrated'.format(self.node_name), GetBool,
self.handle_is_calibrated_srv)
self.get_transform_2d_srv = rospy.Service(
'{0}/get_transform_2d'.format(self.node_name), GetTransform2d,
self.handle_get_transform_2d_srv)
self.ready = True