def line_detection(LineDetectorClass, bgr):
detector = LineDetectorClass()
detector.setImage(bgr)
image_with_lines = bgr.copy()
# detect lines and normals
white = detector.detectLines("white")
yellow = detector.detectLines("yellow")
red = detector.detectLines("red")
# draw lines
drawLines(image_with_lines, white.lines, (0, 0, 0))
drawLines(image_with_lines, yellow.lines, (255, 0, 0))
drawLines(image_with_lines, red.lines, (0, 255, 0))
# elif isinstance(detector, LineDetector2):
# # detect lines and normals
# lines_white, normals_white, centers_white, area_white = detector.detectLines2('white')
# lines_yellow, normals_yellow, centers_yellow, area_yellow = detector.detectLines2('yellow')
# lines_red, normals_red, centers_red, area_red = detector.detectLines2('red')
#
# # draw lines
# drawLines(image_with_lines, lines_white, (0, 0, 0))
# drawLines(image_with_lines, lines_yellow, (255, 0, 0))
# drawLines(image_with_lines, lines_red, (0, 255, 0))
#
# draw normals
# detector.drawNormals2(centers_white, normals_white, (0, 0, 0))
# detector.drawNormals2(centers_yellow, normals_yellow, (255, 0, 0))
# detector.drawNormals2(centers_red, normals_red, (0, 255, 0))
res = {}
res["annotated"] = image_with_lines
res["area_white"] = white.area
res["area_red"] = red.area
res["area_yellow"] = yellow.area
res["edges"] = detector.edges
return res
def line_detection(LineDetectorClass, bgr):
detector = LineDetectorClass()
detector.setImage(bgr)
image_with_lines = bgr.copy()
# detect lines and normals
white = detector.detectLines('white')
yellow = detector.detectLines('yellow')
red = detector.detectLines('red')
# draw lines
drawLines(image_with_lines, white.lines, (0, 0, 0))
drawLines(image_with_lines, yellow.lines, (255, 0, 0))
drawLines(image_with_lines, red.lines, (0, 255, 0))
# elif isinstance(detector, LineDetector2):
# # detect lines and normals
# lines_white, normals_white, centers_white, area_white = detector.detectLines2('white')
# lines_yellow, normals_yellow, centers_yellow, area_yellow = detector.detectLines2('yellow')
# lines_red, normals_red, centers_red, area_red = detector.detectLines2('red')
#
# # draw lines
# drawLines(image_with_lines, lines_white, (0, 0, 0))
# drawLines(image_with_lines, lines_yellow, (255, 0, 0))
# drawLines(image_with_lines, lines_red, (0, 255, 0))
#
# draw normals
#detector.drawNormals2(centers_white, normals_white, (0, 0, 0))
#detector.drawNormals2(centers_yellow, normals_yellow, (255, 0, 0))
#detector.drawNormals2(centers_red, normals_red, (0, 255, 0))
res = {}
res['annotated'] = image_with_lines
res['area_white'] = white.area
res['area_red'] = red.area
res['area_yellow'] = yellow.area
res['edges'] = detector.edges
return res
def processImage_(self, image_msg):
self.stats.processed()
if self.intermittent_log_now():
self.intermittent_log(self.stats.info())
self.stats.reset()
tk = TimeKeeper(image_msg)
self.intermittent_counter += 1
# Decode from compressed image with OpenCV
try:
image_cv = bgr_from_jpg(image_msg.data)
except ValueError as e:
self.loginfo('Could not decode image: %s' % e)
return
tk.completed('decoded')
# Resize and crop image
hei_original, wid_original = image_cv.shape[0:2]
if self.image_size[0] != hei_original or self.image_size[
1] != wid_original:
# image_cv = cv2.GaussianBlur(image_cv, (5,5), 2)
image_cv = cv2.resize(image_cv,
(self.image_size[1], self.image_size[0]),
interpolation=cv2.INTER_NEAREST)
image_cv = image_cv[self.top_cutoff:, :, :]
tk.completed('resized')
# Set the image to be detected
self.detector_used.setImage(image_cv)
# Detect lines and normals
#rospy.loginfo('segment_max_threshold: %s' % self.segment_max_threshold)
white = self.detector_used.detectLines('white',
self.segment_max_threshold)
yellow = self.detector_used.detectLines('yellow',
self.segment_max_threshold)
red = self.detector_used.detectLines('red', self.segment_max_threshold)
tk.completed('detected')
# SegmentList constructor
segmentList = SegmentList()
segmentList.header.stamp = image_msg.header.stamp
# Convert to normalized pixel coordinates, and add segments to segmentList
arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff))
arr_ratio = np.array(
(1. / self.image_size[1], 1. / self.image_size[0],
1. / self.image_size[1], 1. / self.image_size[0]))
if len(white.lines) > 0:
lines_normalized_white = ((white.lines + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_white, white.normals,
Segment.WHITE))
if len(yellow.lines) > 0:
lines_normalized_yellow = ((yellow.lines + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_yellow, yellow.normals,
Segment.YELLOW))
if len(red.lines) > 0:
lines_normalized_red = ((red.lines + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_red, red.normals,
Segment.RED))
self.intermittent_log(
'# segments: white %3d yellow %3d red %3d' %
(len(white.lines), len(yellow.lines), len(red.lines)))
tk.completed('prepared')
# Publish segmentList
#rospy.loginfo('segmentList: %s' % (len(white.lines)+len(yellow.lines)+len(red.lines)))
self.pub_lines.publish(segmentList)
tk.completed('--pub_lines--')
# VISUALIZATION only below
if self.verbose:
# print('line_detect_node: verbose is on!')
# Draw lines and normals
image_with_lines = np.copy(image_cv)
drawLines(image_with_lines, white.lines, (0, 0, 0))
drawLines(image_with_lines, yellow.lines, (255, 0, 0))
drawLines(image_with_lines, red.lines, (0, 255, 0))
tk.completed('drawn')
# Publish the frame with lines
image_msg_out = self.bridge.cv2_to_imgmsg(image_with_lines, "bgr8")
image_msg_out.header.stamp = image_msg.header.stamp
self.pub_image.publish(image_msg_out)
tk.completed('pub_image')
# if self.verbose:
colorSegment = color_segment(white.area, red.area, yellow.area)
edge_msg_out = self.bridge.cv2_to_imgmsg(self.detector_used.edges,
"mono8")
colorSegment_msg_out = self.bridge.cv2_to_imgmsg(
colorSegment, "bgr8")
self.pub_edge.publish(edge_msg_out)
self.pub_colorSegment.publish(colorSegment_msg_out)
tk.completed('pub_edge/pub_segment')
self.intermittent_log(tk.getall())
def cbImage(self, image_msg):
# self.sub_rect.unregister()
image_cv = self.bridge.imgmsg_to_cv2(image_msg,
desired_encoding="bgr8")
hei_original, wid_original = image_cv.shape[0:2]
if self.image_size[0] != hei_original or self.image_size[
1] != wid_original:
# image_cv = cv2.GaussianBlur(image_cv, (5,5), 2)
image_cv = cv2.resize(image_cv,
(self.image_size[1], self.image_size[0]),
interpolation=cv2.INTER_NEAREST)
image_cv = image_cv[self.top_cutoff:, :, :]
# Set the image to be detected
self.detector_used.setImage(image_cv)
# Detect lines and normals
white = self.detector_used.detectLines('white')
yellow = self.detector_used.detectLines('yellow')
red = self.detector_used.detectLines('red')
# SegmentList constructor
segmentList = SegmentList()
segmentList.header.stamp = image_msg.header.stamp
# Convert to normalized pixel coordinates, and add segments to segmentList
arr_cutoff = np.array((0, self.top_cutoff, 0, self.top_cutoff))
arr_ratio = np.array(
(1. / self.image_size[1], 1. / self.image_size[0],
1. / self.image_size[1], 1. / self.image_size[0]))
if len(white.lines) > 0:
lines_normalized_white = ((white.lines + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_white, white.normals,
Segment.WHITE))
if len(yellow.lines) > 0:
lines_normalized_yellow = ((yellow.lines + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_yellow, yellow.normals,
Segment.YELLOW))
if len(red.lines) > 0:
lines_normalized_red = ((red.lines + arr_cutoff) * arr_ratio)
segmentList.segments.extend(
self.toSegmentMsg(lines_normalized_red, red.normals,
Segment.RED))
rospy.loginfo('# segments: white %3d yellow %3d red %3d' %
(len(white.lines), len(yellow.lines), len(red.lines)))
# Publish segmentList
self.pub_lines.publish(segmentList)
if self.verbose:
# print('line_detect_node: verbose is on!')
# Draw lines and normals
image_with_lines = np.copy(image_cv)
drawLines(image_with_lines, white.lines, (0, 0, 0))
drawLines(image_with_lines, yellow.lines, (255, 0, 0))
drawLines(image_with_lines, red.lines, (0, 255, 0))
# Publish the frame with lines
image_msg_out = self.bridge.cv2_to_imgmsg(image_with_lines, "bgr8")
image_msg_out.header.stamp = image_msg.header.stamp
self.pub_image.publish(image_msg_out)
colorSegment = color_segment(white.area, red.area, yellow.area)
edge_msg_out = self.bridge.cv2_to_imgmsg(self.detector_used.edges,
"mono8")
colorSegment_msg_out = self.bridge.cv2_to_imgmsg(
colorSegment, "bgr8")
self.pub_edge.publish(edge_msg_out)
self.pub_colorSegment.publish(colorSegment_msg_out)
def cbImage(self,image_msg):
# self.sub_rect.unregister()
image_cv = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding="bgr8")
hei_original, wid_original = image_cv.shape[0:2]
if self.image_size[0] != hei_original or self.image_size[1] != wid_original:
# image_cv = cv2.GaussianBlur(image_cv, (5,5), 2)
image_cv = cv2.resize(image_cv, (self.image_size[1], self.image_size[0]),
interpolation=cv2.INTER_NEAREST)
image_cv = image_cv[self.top_cutoff:,:,:]
# Set the image to be detected
self.detector_used.setImage(image_cv)
# Detect lines and normals
white = self.detector_used.detectLines('white')
yellow = self.detector_used.detectLines('yellow')
red = self.detector_used.detectLines('red')
# print(white.lines)
white_c = len(white.lines)
yellow_c = len(yellow.lines)
red_c = len(red.lines)
# white_lane = np.zeros(3)
# yellow_lane = np.zeros(3)
# red_lane = np.zeros(3)
white_lane = [0,0,0]
yellow_lane = [0,0,0]
red_lane = [0,0,0]
if white_c > 0:
white_lane = self.detectLane2NoWeights(white)
if yellow_c > 0 :
yellow_lane = self.detectLane2NoWeights(yellow)
if red_c > 0 :
red_lane = self.detectLane2NoWeights(red)
self.intermittent_counter += 1
# self.drive(white_lane,yellow_lane,red_lane)
print('white d:%6f phi:%6f n:%3d , yellow d:%6f phi:%6f n:%3d , red d:%6f phi:%6f n:%3d ' % (white_lane[0],white_lane[1],
white_c,yellow_lane[0],yellow_lane[1], yellow_c, red_lane[0],red_lane[1] , red_c))
if self.verbose:
# print('line_detect_node: verbose is on!')
# Draw lines and normals
image_with_lines = np.copy(image_cv)
drawLines(image_with_lines, white.lines, (0, 0, 0))
drawLines(image_with_lines, yellow.lines, (255, 0, 0))
drawLines(image_with_lines, red.lines, (0, 255, 0))
# Publish the frame with lines
image_msg_out = self.bridge.cv2_to_imgmsg(image_with_lines, "bgr8")
image_msg_out.header.stamp = image_msg.header.stamp
self.pub_image.publish(image_msg_out)
colorSegment = color_segment(white.area, red.area, yellow.area)
edge_msg_out = self.bridge.cv2_to_imgmsg(self.detector_used.edges, "mono8")
colorSegment_msg_out = self.bridge.cv2_to_imgmsg(colorSegment, "bgr8")
self.pub_edge.publish(edge_msg_out)
self.pub_colorSegment.publish(colorSegment_msg_out)
