class AntiInstagramNode():
def __init__(self):
self.node_name = rospy.get_name()
self.active = True
self.locked = False
self.image_pub_switch = rospy.get_param("~publish_corrected_image",
False)
# Initialize publishers and subscribers
self.pub_image = rospy.Publisher("~corrected_image",
Image,
queue_size=1)
self.pub_health = rospy.Publisher("~health",
AntiInstagramHealth,
queue_size=1,
latch=True)
self.pub_transform = rospy.Publisher("~transform",
AntiInstagramTransform,
queue_size=1,
latch=True)
#self.sub_switch = rospy.Subscriber("~switch",BoolStamped, self.cbSwitch, queue_size=1)
#self.sub_image = rospy.Subscriber("~uncorrected_image",Image,self.cbNewImage,queue_size=1)
self.sub_image = rospy.Subscriber("~uncorrected_image",
CompressedImage,
self.cbNewImage,
queue_size=1)
self.sub_click = rospy.Subscriber("~click",
BoolStamped,
self.cbClick,
queue_size=1)
self.trans_timer = rospy.Timer(rospy.Duration.from_sec(20),
self.cbPubTrans, True)
# Verbose option
self.verbose = rospy.get_param('line_detector_node/verbose', True)
# Initialize health message
self.health = AntiInstagramHealth()
# Initialize transform message
self.transform = AntiInstagramTransform()
# FIXME: read default from configuration and publish it
self.ai_scale = np.array(
[2.2728408473337893, 2.2728273205024614, 2.272844346401005])
self.ai_shift = np.array(
[21.47181119272393, 37.14653160247276, 4.089311860796786])
self.ai = AntiInstagram()
self.corrected_image = Image()
self.bridge = CvBridge()
self.image_msg = None
self.click_on = False
def cbPubTrans(self, _):
self.transform.s[0], self.transform.s[1], self.transform.s[
2] = self.ai_shift
self.transform.s[3], self.transform.s[4], self.transform.s[
5] = self.ai_scale
self.pub_transform.publish(self.transform)
rospy.loginfo('ai: Color transform published.')
def cbNewImage(self, image_msg):
# memorize image
self.image_msg = image_msg
if self.image_pub_switch:
tk = TimeKeeper(image_msg)
cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
corrected_image_cv2 = self.ai.applyTransform(cv_image)
tk.completed('applyTransform')
corrected_image_cv2 = np.clip(corrected_image_cv2, 0,
255).astype(np.uint8)
self.corrected_image = self.bridge.cv2_to_imgmsg(
corrected_image_cv2, "bgr8")
tk.completed('encode')
self.pub_image.publish(self.corrected_image)
tk.completed('published')
if self.verbose:
rospy.loginfo('ai:\n' + tk.getall())
def cbClick(self, _):
# if we have seen an image:
if self.image_msg is not None:
self.click_on = not self.click_on
if self.click_on:
self.processImage(self.image_msg)
else:
self.transform.s = [0, 0, 0, 1, 1, 1]
self.pub_transform.publish(self.transform)
rospy.loginfo('ai: Color transform is turned OFF!')
def processImage(self, msg):
'''
Inputs:
msg - CompressedImage - uncorrected image from raspberry pi camera
Uses anti_instagram library to adjust msg so that it looks like the same
color temperature as a duckietown reference image. Calculates health of the node
and publishes the corrected image and the health state. Health somehow corresponds
to how good of a transformation it is.
'''
rospy.loginfo('ai: Computing color transform...')
tk = TimeKeeper(msg)
#cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
try:
cv_image = image_cv_from_jpg(msg.data)
except ValueError as e:
rospy.loginfo('Anti_instagram cannot decode image: %s' % e)
return
tk.completed('converted')
self.ai.calculateTransform(cv_image)
tk.completed('calculateTransform')
# if health is much below the threshold value, do not update the color correction and log it.
if self.ai.health <= 0.001:
# health is not good
rospy.loginfo("Health is not good")
else:
self.health.J1 = self.ai.health
self.transform.s[0], self.transform.s[1], self.transform.s[
2] = self.ai.shift
self.transform.s[3], self.transform.s[4], self.transform.s[
5] = self.ai.scale
rospy.set_param('antiins_shift', self.ai.shift.tolist())
rospy.set_param('antiins_scale', self.ai.scale.tolist())
self.pub_health.publish(self.health)
self.pub_transform.publish(self.transform)
rospy.loginfo('ai: Color transform published.')
def main():
# check number of arguments
if len(sys.argv) != 3:
print(
'This program expects exactly two arguments: an image filename and an output directory.'
)
sys.exit()
# store inputs
file = sys.argv[1]
outdir = sys.argv[2]
# check if file exists
if not os.path.isfile(file):
print('file not found')
sys.exit(2)
# check if dir exists, create if not
if not os.path.exists(outdir):
os.makedirs(outdir)
# read the image
input_img = cv2.imread(file, cv2.IMREAD_UNCHANGED)
# lets do the masking!
surf = identifyLaneSurface(input_img, use_hsv=False, grad_thresh=20)
input_img = np.expand_dims(surf, -1) * input_img
print('masked image!')
cv2.imshow('mask', input_img)
cv2.waitKey(0)
# create instance of AntiInstagram
ai = AntiInstagram()
ai.calculateTransform(input_img)
print('Transform Calculation completed!')
corrected_img = ai.applyTransform(input_img)
print('Transform applied!')
cv2.imshow('mask', corrected_img)
cv2.waitKey(0)
# polygons for pic1_smaller.jpg and pic2_smaller.jpg (hardcoded for now)
polygon_black = [(280, 570), (220, 760), (870, 750), (810, 580)]
polygon_white = [(781, 431), (975, 660), (1040, 633), (827, 418)]
polygon_yellow = [(131, 523), (67, 597), (99, 609), (161, 530)]
polygon_red = [(432, 282), (418, 337), (577, 338), (568, 283)]
"""
# polygons for pic3_smaller.jpg and pic2_smaller.jpg (hardcoded for now)
polygon_black = [(280, 570), (220, 760), (870, 750), (810, 580)]
polygon_white = [(900, 520), (1000, 640), (1060, 620), (970, 515)]
polygon_yellow = [(234, 430), (190, 485), (230, 490), (270, 430)]
polygon_red = [(285, 435), (250, 490), (830, 480), (800, 437)]
# polygons for pic4_smaller.jpg (hardcoded for now)
polygon_black = [(316, 414), (215, 623), (783, 605), (673, 422)]
polygon_white = [(710, 388), (947, 656), (1018, 620), (788, 400)]
polygon_yellow = [(148, 474), (94, 537), (133, 542), (184, 475)]
polygon_red = [(285, 435), (250, 490), (830, 480), (800, 437)]
# polygons for pic5_smaller.jpg (hardcoded for now)
polygon_black = [(354, 418), (291, 612), (804, 590), (677, 396)]
polygon_white = [(783, 424), (949, 602), (1002, 564), (840, 420)]
polygon_yellow = [(344, 307), (331, 319), (354, 319), (366, 306)]
polygon_red = [(135, 325), (119, 332), (325, 316), (332, 309)]
"""
# create dictionary containing colors
polygons = {
'black': polygon_black,
'white': polygon_white,
'yellow': polygon_yellow,
'red': polygon_red
}
# initialize estimateError class
E = estimateError(corrected_img)
# set polygons
E.createPolygon(polygons)
# estimate error
E.GetErrorEstimation()
# write the corrected image
date = datetime.datetime.now().strftime("%H-%M-%S")
path = outdir + '/' + str(date) + '_polygon.jpg'
cv2.imwrite(path, E.out_image)
class AntiInstagramNode():
def __init__(self):
self.node_name = rospy.get_name()
self.active = True
self.image_pub_switch = rospy.get_param("~publish_corrected_image",False)
# Initialize publishers and subscribers
self.pub_image = rospy.Publisher("~corrected_image",Image,queue_size=1)
self.pub_health = rospy.Publisher("~health",AntiInstagramHealth,queue_size=1,latch=True)
self.pub_transform = rospy.Publisher("~transform",AntiInstagramTransform,queue_size=1,latch=True)
#self.sub_switch = rospy.Subscriber("~switch",BoolStamped, self.cbSwitch, queue_size=1)
#self.sub_image = rospy.Subscriber("~uncorrected_image",Image,self.cbNewImage,queue_size=1)
self.sub_image = rospy.Subscriber("~uncorrected_image", CompressedImage, self.cbNewImage,queue_size=1)
self.sub_click = rospy.Subscriber("~click", BoolStamped, self.cbClick, queue_size=1)
# Verbose option
self.verbose = rospy.get_param('~verbose',True)
# Initialize health message
self.health = AntiInstagramHealth()
# Initialize transform message
self.transform = AntiInstagramTransform()
# FIXME: read default from configuration and publish it
self.ai = AntiInstagram()
self.corrected_image = Image()
self.bridge = CvBridge()
self.image_msg = None
self.click_on = False
def cbNewImage(self,image_msg):
# memorize image
self.image_msg = image_msg
if self.image_pub_switch:
tk = TimeKeeper(image_msg)
cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8")
corrected_image_cv2 = self.ai.applyTransform(cv_image)
tk.completed('applyTransform')
corrected_image_cv2 = np.clip(corrected_image_cv2, 0, 255).astype(np.uint8)
self.corrected_image = self.bridge.cv2_to_imgmsg(corrected_image_cv2, "bgr8")
tk.completed('encode')
self.pub_image.publish(self.corrected_image)
tk.completed('published')
if self.verbose:
rospy.loginfo('ai:\n' + tk.getall())
def cbClick(self, _):
# if we have seen an image:
if self.image_msg is not None:
self.click_on = not self.click_on
if self.click_on:
self.processImage(self.image_msg)
else:
self.transform.s = [0,0,0,1,1,1]
self.pub_transform.publish(self.transform)
rospy.loginfo('ai: Color transform is turned OFF!')
else:
rospy.loginfo('No image seen yet')
def processImage(self,msg):
'''
Inputs:
msg - CompressedImage - uncorrected image from raspberry pi camera
Uses anti_instagram library to adjust msg so that it looks like the same
color temperature as a duckietown reference image. Calculates health of the node
and publishes the corrected image and the health state. Health somehow corresponds
to how good of a transformation it is.
'''
rospy.loginfo('ai: Computing color transform...')
tk = TimeKeeper(msg)
#cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
try:
cv_image = image_cv_from_jpg(msg.data)
except ValueError as e:
rospy.loginfo('Anti_instagram cannot decode image: %s' % e)
return
tk.completed('converted')
self.ai.calculateTransform(cv_image)
tk.completed('calculateTransform')
# FIXME !!!
if False:
# health is not good
rospy.loginfo("Health is not good")
else:
self.health.J1 = self.ai.health
self.transform.s[0], self.transform.s[1], self.transform.s[2] = self.ai.shift
self.transform.s[3], self.transform.s[4], self.transform.s[5] = self.ai.scale
self.pub_health.publish(self.health)
self.pub_transform.publish(self.transform)
rospy.loginfo('ai: Color transform published!!!')
os.makedirs(outdir)
# read the image
input_img = cv2.imread(file, cv2.IMREAD_UNCHANGED)
#lets do the masking!
#surf = identifyLaneSurface(input_img, use_hsv=False, grad_thresh=20)
#input_img = np.expand_dims(surf, -1) * input_img
#cv2.imshow('mask', input_img)
#cv2.waitKey(0)
# create instance of AntiInstagram
ai = AntiInstagram()
ai.calculateTransform(input_img)
print('Transform Calculation completed!')
corrected_img = ai.applyTransform(input_img)
print('Transform applied!')
# write the corrected image
date = datetime.datetime.now().strftime("%H-%M-%S")
path = outdir + '/' + str(date) + '_corrected.jpg'
cv2.imwrite(path, corrected_img)
print('calculated scale: ' + str(ai.scale))
print('calculated shift: ' + str(ai.shift))
