class LaneDetector:
def __init__(self, node_name, sub_topic, pub_topic):
self.bridge = CvBridge()
self.img_proc = ImagePreprocessor()
# Publishers
#self.
rospy.init_node(node_name)
# Crop parameters
self.above = rospy.get_param("/asv_lane_track/lane_detection_node/above", 0.35)
self.below = rospy.get_param("/asv_lane_track/lane_detection_node/below", 0.1)
self.side = rospy.get_param("/asv_lane_track/lane_detection_node/side", 0)
# Lane tracking parameters
self.deviation = rospy.get_param("/asv_lane_track/lane_detection_node/deviation", 15)
self.border = rospy.get_param("/asv_lane_track/lane_detection_node/border", 0)
# Canny parameters
self.low_thresh = rospy.get_param("/asv_lane_track/lane_detection_node/low_threshold", 50)
self.high_thresh = rospy.get_param("/asv_lane_track/lane_detection_node/high_threshold", 150)
self.aperture = rospy.get_param("/asv_lane_track/lane_detection_node/aperture", 3)
self.image_sub = rospy.Subscriber(sub_topic, Image, self.img_callback)
self.image_pub = rospy.Publisher(pub_topic, Image, queue_size=5)
self.yellow_masked_pub = rospy.Publisher("/yellow_masked", Image, queue_size=5)
self.gray_pub = rospy.Publisher("/test", Image, queue_size=5)
self.blur_pub = rospy.Publisher("/blur", Image, queue_size=5)
self.left_lines = deque(maxlen=QUEUE_LENGTH)
self.right_lines = deque(maxlen=QUEUE_LENGTH)
rospy.spin()
def process(self, img):
cropped = self.img_proc.crop(img, self.above, self.below, self.side)
mask = cropped
yellow_mask = self.img_proc.select_yellow(mask)
yellow_img = cv2.bitwise_and(cropped, cropped, mask=yellow_mask)
gray = self.img_proc.convert_gray(yellow_img)
smooth_gray = self.img_proc.blur(gray, (self.deviation, self.deviation), self.border)
edges = self.img_proc.edge_detection(smooth_gray, self.low_thresh, self.high_thresh, self.aperture)
lines = self.img_proc.hough_lines(edges)
# print(lines)
return self.img_proc.draw_lines_from_points(cropped, lines)
# left_line, right_line = self.img_proc.lane_lines(cropped, lines)
#
# def mean_line(line, lines):
# if line is not None:
# lines.append(line)
#
# if len(lines) > 0:
# line = np.mean(lines, axis=0, dtype=np.int32)
# line = tuple(map(tuple, line)) # make sure it's tuples not numpy array for cv2.line to work
# return line
#
# left_line = mean_line(left_line, self.left_lines)
# right_line = mean_line(right_line, self.right_lines)
#
# middle_line = ((int(left_line[0][0] + (right_line[0][0] - left_line[0][0]) / 2), int(left_line[0][1])),
# (int(left_line[1][0] + (right_line[1][0] - left_line[1][0]) / 2), int(left_line[1][1])))
# return self.img_proc.draw_lane_lines(cropped, (left_line, right_line, middle_line))
def img_callback(self, data):
try:
cv_img = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
rospy.logerr(e)
img_proc = cv_img
# crop
cropped = self.img_proc.crop(img_proc, self.above, self.below, self.side)
# grayscale
#gray = self.img_proc.grayscale(cropped)
# Extract yellow color in hsv
yellow_masked = self.img_proc.select_yellow(cropped)
res = cv2.bitwise_and(cropped,cropped, mask = yellow_masked)
# blur
# uhm why do you choose
blurred = self.img_proc.blur(res, (self.deviation, self.deviation), self.border)
# canny
canny = self.img_proc.edge_detection(blurred, self.low_thresh, self.high_thresh, self.aperture)
# # cv2.imshow("canny", canny)
# # cv2.waitKey(25)
# #canny
# lines = self.img_proc.hough_lines(canny)
# left_line, right_line = self.img_proc.lane_lines(cv_img, lines)
# final = self.img_proc.draw_lane_lines(cv_img, (left_line, right_line))
# for line in lines:
# self.img_proc.draw_lane_lines()
lane_img = self.process(cv_img)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(lane_img, encoding="rgb8"))
self.gray_pub.publish(self.bridge.cv2_to_imgmsg(canny))
#cv2.imshow('mask', cv_img)
# cv2.imshow('mask', res)
#
#cv2.waitKey(25)
#self.yellow_masked_pub.publish(self.bridge.cv2_to_imgmsg(yellow_masked, encoding="rgb8"))
except CvBridgeError as e:
rospy.logerr(e)
