def __init__(self):
self.node_name = rospy.get_name()
self.image_lock = threading.Lock()
self.ai = AntiInstagram()
# XXX: read parameters
# XXX: parameters need to go inside config file, ask aleks about heirarchy
self.interval = self.setup_parameter("~ai_interval", 10)
self.color_balance_percentage = self.setup_parameter("~cb_percentage", 0.8) # XXX: change in all launch files
self.output_scale = self.setup_parameter("~scale_percent", 0.4) # XXX: change in all launch files
self.calculation_scale = self.setup_parameter("~resize", 0.2)
self.bridge = CvBridge()
self.image = None
rospy.Timer(rospy.Duration(self.interval), self.calculate_new_parameters)
self.uncorrected_image_subscriber = rospy.Subscriber(
'~uncorrected_image/compressed',
CompressedImage,
self.process_image,
buff_size=921600,
queue_size=1)
self.corrected_image_publisher = rospy.Publisher(
"~corrected_image/compressed",
CompressedImage,
queue_size=1)
def __init__(self, node_name):
super(AntiInstagramNode, self).__init__(node_name=node_name, parameters_update_period=10.0)
self.veh_name = rospy.get_namespace().strip("/")
# Initialize parameters
self.parameters['~ai_interval'] = None
self.parameters['~cb_percentage'] = None
self.parameters['~scale_percent'] = None
self.parameters['~resize'] = None
self.updateParameters()
self.refresh_parameters()
self.image = None
self.image_lock = threading.Lock()
self.ai = AntiInstagram()
self.bridge = CvBridge()
self.uncorrected_image_subscriber = self.subscriber(
'~uncorrected_image/compressed',
CompressedImage,
self.process_image,
buff_size=921600,
queue_size=1)
self.corrected_image_publisher = self.publisher(
"~corrected_image/compressed",
CompressedImage,
queue_size=1)
rospy.Timer(rospy.Duration(self.interval), self.calculate_new_parameters)
def __init__(self):
EasyNode.__init__(self, 'line_detector2', 'line_detector_node2')
self.detector = None
self.bridge = CvBridge()
self.ai = AntiInstagram()
self.active = True
# Only be verbose every 10 cycles
self.intermittent_interval = 100
self.intermittent_counter = 0
def __init__(self):
self.node_name = "LineDetectorNode"
# Thread lock
self.thread_lock = threading.Lock()
# Constructor of line detector
self.bridge = CvBridge()
self.active = True
self.stats = Stats()
# Only be verbose every 10 cycles
self.intermittent_interval = 100
self.intermittent_counter = 0
# color correction
self.ai = AntiInstagram()
# these will be added if it becomes verbose
self.pub_edge = None
self.pub_colorSegment = None
self.detector = None
self.verbose = None
self.updateParams(None)
# Publishers
self.pub_lines = rospy.Publisher("~segment_list",
SegmentList,
queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines",
Image,
queue_size=1)
# Subscribers
self.sub_image = rospy.Subscriber("~image",
CompressedImage,
self.cbImage,
queue_size=1)
self.sub_transform = rospy.Subscriber("~transform",
AntiInstagramTransform,
self.cbTransform,
queue_size=1)
self.sub_switch = rospy.Subscriber("~switch",
BoolStamped,
self.cbSwitch,
queue_size=1)
rospy.loginfo("[%s] Initialized (verbose = %s)." %
(self.node_name, self.verbose))
rospy.Timer(rospy.Duration.from_sec(2.0), self.updateParams)
class LineDetectorNode2(EasyNode):
def __init__(self):
EasyNode.__init__(self, 'line_detector2', 'line_detector_node2')
self.detector = None
self.bridge = CvBridge()
self.ai = AntiInstagram()
self.active = True
# Only be verbose every 10 cycles
self.intermittent_interval = 100
self.intermittent_counter = 0
def on_parameters_changed(self, _first_time, updated):
if 'verbose' in updated:
self.info('Verbose is now %r' % self.config.verbose)
if 'line_detector' in updated:
db = get_easy_algo_db()
self.detector = db.create_instance('line_detector',
self.config.line_detector)
def on_received_switch(self, context, switch_msg): # @UnusedVariable
self.active = switch_msg.data
def on_received_transform(self, context, transform_msg): # @UnusedVariable
self.ai.shift = transform_msg.s[0:3]
self.ai.scale = transform_msg.s[3:6]
self.info("AntiInstagram transform received")
def on_received_image(self, context, image_msg):
if not self.active:
return
self.intermittent_counter += 1
with context.phase('decoding'):
# Decode from compressed image with OpenCV
try:
image_cv = image_cv_from_jpg(image_msg.data)
except ValueError as e:
self.loginfo('Could not decode image: %s' % e)
return
with context.phase('resizing'):
# Resize and crop image
hei_original, wid_original = image_cv.shape[0:2]
if self.config.img_size[0] != hei_original or self.config.img_size[
1] != wid_original:
# image_cv = cv2.GaussianBlur(image_cv, (5,5), 2)
image_cv = cv2.resize(
image_cv,
(self.config.img_size[1], self.config.img_size[0]),
interpolation=cv2.INTER_NEAREST)
image_cv = image_cv[self.config.top_cutoff:, :, :]
with context.phase('correcting'):
# apply color correction: AntiInstagram
image_cv_corr = self.ai.applyTransform(image_cv)
image_cv_corr = cv2.convertScaleAbs(image_cv_corr)
with context.phase('detection'):
# Set the image to be detected
self.detector.setImage(image_cv_corr)
# Detect lines and normals
white = self.detector.detectLines('white')
yellow = self.detector.detectLines('yellow')
red = self.detector.detectLines('red')
with context.phase('preparing-images'):
# SegmentList constructor
segmentList = SegmentList()
segmentList.header.stamp = image_msg.header.stamp
# Convert to normalized pixel coordinates, and add segments to segmentList
top_cutoff = self.config.top_cutoff
s0, s1 = self.config.img_size[0], self.config.img_size[1]
arr_cutoff = np.array((0, top_cutoff, 0, top_cutoff))
arr_ratio = np.array((1. / s1, 1. / s0, 1. / s1, 1. / s0))
if len(white.lines) > 0:
lines_normalized_white = ((white.lines + arr_cutoff) *
arr_ratio)
segmentList.segments.extend(
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(
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(
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)))
# Publish segmentList
with context.phase('publishing'):
self.publishers.segment_list.publish(segmentList)
# VISUALIZATION only below
if self.config.verbose:
with context.phase('draw-lines'):
# Draw lines and normals
image_with_lines = np.copy(image_cv_corr)
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))
with context.phase('published-images'):
# Publish the frame with lines
out = d8n_image_msg_from_cv_image(image_with_lines,
"bgr8",
same_timestamp_as=image_msg)
self.publishers.image_with_lines.publish(out)
with context.phase('pub_edge/pub_segment'):
out = d8n_image_msg_from_cv_image(self.detector.edges,
"mono8",
same_timestamp_as=image_msg)
self.publishers.edge.publish(out)
colorSegment = color_segment(white.area, red.area, yellow.area)
out = d8n_image_msg_from_cv_image(colorSegment,
"bgr8",
same_timestamp_as=image_msg)
self.publishers.color_segment.publish(out)
if self.intermittent_log_now():
self.info('stats from easy_node\n' +
indent(context.get_stats(), '> '))
def intermittent_log_now(self):
return self.intermittent_counter % self.intermittent_interval == 1
def intermittent_log(self, s):
if not self.intermittent_log_now():
return
self.info('%3d:%s' % (self.intermittent_counter, s))
class LineDetectorNode(object):
def __init__(self):
self.node_name = "LineDetectorNode"
# Thread lock
self.thread_lock = threading.Lock()
# Constructor of line detector
self.bridge = CvBridge()
self.active = True
self.stats = Stats()
# Only be verbose every 10 cycles
self.intermittent_interval = 100
self.intermittent_counter = 0
# color correction
self.ai = AntiInstagram()
# these will be added if it becomes verbose
self.pub_edge = None
self.pub_colorSegment = None
self.detector = None
self.verbose = None
self.updateParams(None)
self.vehicle = rospy.get_param('~vehicle')
# Publishers
# self.pub_lines = rospy.Publisher("~segment_list", SegmentList, queue_size=1)
# self.pub_image = rospy.Publisher("~image_with_lines", Image, queue_size=1)
self.pub_lines = rospy.Publisher("~segment_list_lsd",
SegmentList,
queue_size=1)
self.pub_image = rospy.Publisher("~image_with_lines_lsd",
Image,
queue_size=1)
# Subscribers
# self.sub_image = rospy.Subscriber("~image", CompressedImage, self.cbImage, queue_size=1)
# self.sub_transform = rospy.Subscriber("~transform", AntiInstagramTransform, self.cbTransform, queue_size=1)
# self.sub_switch = rospy.Subscriber("~switch", BoolStamped, self.cbSwitch, queue_size=1)
self.sub_image = rospy.Subscriber("/" + self.vehicle +
"/camera_node/image/compressed",
CompressedImage,
self.cbImage,
queue_size=1)
self.sub_transform = rospy.Subscriber("/" + self.vehicle +
"/anti_instagram/transform",
AntiInstagramTransform,
self.cbTransform,
queue_size=1)
# self.sub_switch = rospy.Subscriber("~switch", BoolStamped, self.cbSwitch, queue_size=1)
rospy.loginfo("[%s] Vehicle name = %s." %
(self.node_name, self.vehicle))
rospy.loginfo("[%s] Initialized (verbose = %s)." %
(self.node_name, self.verbose))
rospy.loginfo("[%s] %s/camera_node/image/compressed" %
(self.node_name, self.vehicle))
rospy.Timer(rospy.Duration.from_sec(2.0), self.updateParams)
def updateParams(self, _event):
old_verbose = self.verbose
self.verbose = rospy.get_param('~verbose', True)
# self.loginfo('verbose = %r' % self.verbose)
if self.verbose != old_verbose:
self.loginfo('Verbose is now %r' % self.verbose)
self.image_size = rospy.get_param('~img_size')
self.top_cutoff = rospy.get_param('~top_cutoff')
if self.detector is None:
c = rospy.get_param('~detector')
assert isinstance(c, list) and len(c) == 2, c
# if str(self.detector_config) != str(c):
self.loginfo('new detector config: %s' % str(c))
self.detector = instantiate(c[0], c[1])
# self.detector_config = c
if self.verbose and self.pub_edge is None:
self.pub_edge = rospy.Publisher("~edge", Image, queue_size=1)
self.pub_colorSegment = rospy.Publisher("~colorSegment",
Image,
queue_size=1)
def cbSwitch(self, switch_msg):
self.active = switch_msg.data
def cbImage(self, image_msg):
self.stats.received()
if not self.active:
return
# Start a daemon thread to process the image
thread = threading.Thread(target=self.processImage, args=(image_msg, ))
thread.setDaemon(True)
thread.start()
# Returns rightaway
def cbTransform(self, transform_msg):
self.ai.shift = transform_msg.s[0:3]
self.ai.scale = transform_msg.s[3:6]
self.loginfo("AntiInstagram transform received")
def loginfo(self, s):
rospy.loginfo('[%s] %s' % (self.node_name, s))
def intermittent_log_now(self):
return self.intermittent_counter % self.intermittent_interval == 1
def intermittent_log(self, s):
if not self.intermittent_log_now():
return
self.loginfo('%3d:%s' % (self.intermittent_counter, s))
def processImage(self, image_msg):
if not self.thread_lock.acquire(False):
self.stats.skipped()
# Return immediately if the thread is locked
return
try:
self.processImage_(image_msg)
finally:
# Release the thread lock
self.thread_lock.release()
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 = image_cv_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')
# apply color correction: AntiInstagram
image_cv_corr = self.ai.applyTransform(image_cv)
image_cv_corr = cv2.convertScaleAbs(image_cv_corr)
tk.completed('corrected')
# Set the image to be detected
self.detector.setImage(image_cv_corr)
# Detect lines and normals
white = self.detector.detectLines('white')
yellow = self.detector.detectLines('yellow')
red = self.detector.detectLines('red')
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
self.pub_lines.publish(segmentList)
tk.completed('--pub_lines--')
# VISUALIZATION only below
#if self.verbose:
# Draw lines and normals
image_with_lines = np.copy(image_cv_corr)
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.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 onShutdown(self):
self.loginfo("Shutdown.")
def toSegmentMsg(self, lines, normals, color):
segmentMsgList = []
for x1, y1, x2, y2, norm_x, norm_y in np.hstack((lines, normals)):
segment = Segment()
segment.color = color
segment.pixels_normalized[0].x = x1
segment.pixels_normalized[0].y = y1
segment.pixels_normalized[1].x = x2
segment.pixels_normalized[1].y = y2
segment.normal.x = norm_x
segment.normal.y = norm_y
segmentMsgList.append(segment)
return segmentMsgList
class AntiInstagramNode():
def __init__(self):
self.node_name = rospy.get_name()
self.image_lock = threading.Lock()
self.ai = AntiInstagram()
# XXX: read parameters
# XXX: parameters need to go inside config file, ask aleks about heirarchy
self.interval = self.setup_parameter("~ai_interval", 10)
self.color_balance_percentage = self.setup_parameter("~cb_percentage", 0.8) # XXX: change in all launch files
self.output_scale = self.setup_parameter("~scale_percent", 0.4) # XXX: change in all launch files
self.calculation_scale = self.setup_parameter("~resize", 0.2)
self.bridge = CvBridge()
self.image = None
rospy.Timer(rospy.Duration(self.interval), self.calculate_new_parameters)
self.uncorrected_image_subscriber = rospy.Subscriber(
'~uncorrected_image/compressed',
CompressedImage,
self.process_image,
buff_size=921600,
queue_size=1)
self.corrected_image_publisher = rospy.Publisher(
"~corrected_image/compressed",
CompressedImage,
queue_size=1)
def process_image(self, image_msg):
try:
self.image_lock.acquire()
image = bgr_from_jpg(image_msg.data)
self.image = image
self.image_lock.release()
except ValueError as e:
rospy.loginfo('Anti_instagram cannot decode image: %s' % e)
self.image_lock.release()
return
color_balanced_image = self.ai.apply_color_balance(image,
self.output_scale)
if color_balanced_image is None:
self.calculate_new_parameters(None)
return
corrected_image = self.bridge.cv2_to_compressed_imgmsg(
color_balanced_image)
corrected_image.header.stamp = image_msg.header.stamp
self.corrected_image_publisher.publish(corrected_image)
def calculate_new_parameters(self, event):
self.image_lock.acquire()
image = self.image
self.image_lock.release()
if image is None:
rospy.loginfo("[%s] Waiting for first image!" % self.node_name)
return
self.ai.calculate_color_balance_thresholds(image,
self.calculation_scale,
self.color_balance_percentage)
rospy.loginfo("[%s] New parameters computed" % self.node_name)
def setup_parameter(self, param_name, default_value):
value = rospy.get_param(param_name, default_value)
rospy.set_param(param_name, value)
rospy.loginfo("[%s] %s = %s " % (self.node_name, param_name, value))
return value
class AntiInstagramNode(DTROS):
def __init__(self, node_name):
super(AntiInstagramNode, self).__init__(node_name=node_name, parameters_update_period=10.0)
self.veh_name = rospy.get_namespace().strip("/")
# Initialize parameters
self.parameters['~ai_interval'] = None
self.parameters['~cb_percentage'] = None
self.parameters['~scale_percent'] = None
self.parameters['~resize'] = None
self.updateParameters()
self.refresh_parameters()
self.image = None
self.image_lock = threading.Lock()
self.ai = AntiInstagram()
self.bridge = CvBridge()
self.uncorrected_image_subscriber = self.subscriber(
'~uncorrected_image/compressed',
CompressedImage,
self.process_image,
buff_size=921600,
queue_size=1)
self.corrected_image_publisher = self.publisher(
"~corrected_image/compressed",
CompressedImage,
queue_size=1)
rospy.Timer(rospy.Duration(self.interval), self.calculate_new_parameters)
def refresh_parameters(self):
self.interval = self.parameters["~ai_interval"]
self.color_balance_percentage = self.parameters["~cb_percentage"]
self.output_scale = self.parameters["~resize"]
self.calculation_scale = self.parameters["~scale_percent"]
def process_image(self, image_msg):
if self.parametersChanged:
self.log('Parameters changed.', 'info')
self.refresh_parameters()
self.parametersChanged = False
try:
self.image_lock.acquire()
image = bgr_from_jpg(image_msg.data)
self.image = image
self.image_lock.release()
except ValueError as e:
self.log('Anti_instagram cannot decode image: %s' % e)
self.image_lock.release()
return
color_balanced_image = self.ai.apply_color_balance(image,
self.output_scale)
if color_balanced_image is None:
self.calculate_new_parameters(None)
return
corrected_image = self.bridge.cv2_to_compressed_imgmsg(
color_balanced_image)
corrected_image.header.stamp = image_msg.header.stamp
self.corrected_image_publisher.publish(corrected_image)
def calculate_new_parameters(self, event=None):
self.image_lock.acquire()
image = self.image
self.image_lock.release()
if image is None:
self.log("[%s] Waiting for first image!" % self.node_name)
return
self.ai.calculate_color_balance_thresholds(image,
self.calculation_scale,
self.color_balance_percentage)
self.log("[%s] New parameters computed" % self.node_name)
def onShutdown(self):
self.log("Stopping preprocessor_node.")
super(PreprocessorNode, self).onShutdown()
def run_pipeline(image, line_detector_name, image_prep_name):
"""
Image: numpy (H,W,3) == BGR
Returns a dictionary, res with the following fields:
res['input_image']
"""
res = OrderedDict()
res['image_input'] = image
algo_db = get_easy_algo_db()
line_detector = algo_db.create_instance('line_detector',
line_detector_name)
image_prep = algo_db.create_instance('image_prep', image_prep_name)
context = FakeContext()
segment_list = image_prep.process(context,
image,
line_detector,
transform=None)
res['segments_on_image_input'] = vs_fancy_display(image_prep.image_cv,
segment_list)
res['segments_on_image_resized'] = vs_fancy_display(
image_prep.image_resized, segment_list)
ai = AntiInstagram()
ai.calculateTransform(res['image_input'])
transform = ai.applyTransform
res['image_input_transformed'] = transform(res['image_input'])
res['image_input_transformed_then_convertScaleAbs'] = cv2.convertScaleAbs(
res['image_input_transformed'])
segment_list2 = image_prep.process(context,
image,
line_detector,
transform=transform)
res['segments2_on_image_input_transformed'] = \
vs_fancy_display(image_prep.image_cv, segment_list2)
res['segments2_on_image_input_transformed_resized'] = \
vs_fancy_display(image_prep.image_resized, segment_list2)
filename = (get_ros_package_path('duckietown') +
"/config/baseline/calibration/camera_intrinsic/default.yaml")
ci = load_camera_info(filename)
# (h,w) = image.shape[:2]
ci_W = ci.width
ci_H = ci.height
mapx, mapy = cv2.initUndistortRectifyMap(ci.K, ci.D, ci.R, ci.P,
(ci_W, ci_H), cv2.CV_32FC1)
# mapx and mapy are (h, w) matrices that tell you
# the x coordinate and the y coordinate for each point
# in the first image
# print mapx.shape(),mapy.shape()
res['grid'] = get_grid(image.shape[:2])
res['grid_remap'] = cv2.remap(res['grid'], mapx, mapy, cv2.INTER_LINEAR)
res['image_input_rect'] = cv2.remap(res['image_input'], mapx, mapy,
cv2.INTER_LINEAR)
segment_list_rect = apply_transform_to_segment(segment_list, mapx, mapy)
res['segments2_on_image_input_rect'] = \
vs_fancy_display(res['image_input_rect'], segment_list_rect)
res['grid_undistort'] = cv2.undistort(res['grid'], ci.K, ci.D)
res['image_input_undistort'] = cv2.undistort(res['image_input'], ci.K,
ci.D)
homography = np.array([
-4.89775e-05, -0.0002150858, -0.1818273, 0.00099274, 1.202336e-06,
-0.3280241, -0.0004281805, -0.007185673, 1
]).reshape((3, 3))
segment_list_undistort = undistort_segments(segment_list,
ci_W,
ci_H,
ci.K,
ci.D,
ci.P,
ci.R,
homography=homography)
res['segments_und_image_input_und'] = vs_fancy_display(
res['image_input_undistort'], segment_list_undistort)
# homography_inv = np.linalg.inv(homography)
res['image_input_undistort_warp'] = cv2.warpPerspective(
res['image_input'],
homography, (ci_W, ci_H),
flags=cv2.WARP_INVERSE_MAP)
# add flags=cv2.WARP_INVERSE_MAP to do the inverse
return res