def on_enter_tile(self, *args):
segs = lineseg_circle(self._flyx, self._flyy, 50)
pts = []
for seg in segs:
pts.append(geometry_msgs.msg.Point32(seg[0], seg[1], 0))
self._pubpts.publish(pts)
def on_enter_tile(self, *args):
segs = lineseg_circle(self._flyx,self._flyy,50)
pts = []
for seg in segs:
pts.append( geometry_msgs.msg.Point32(seg[0],seg[1],0) )
self._pubpts.publish(pts)
def process_frame(self, cam_id, buf, buf_offset, timestamp, framenumber):
"""do work on each frame
This function gets called on every single frame capture. It is
called within the realtime thread, NOT the wxPython
application mainloop's thread. Therefore, be extremely careful
(use threading locks) when sharing data with the rest of the
class.
"""
assert self.pixel_format == "MONO8"
now = time.time()
if self.pub_image is not None:
if (now - self.pub_last_image) > 30.0:
msg = self.pub_image_class()
msg.header.seq = framenumber
# XXX TODO: once camera trigger is ROS node, get accurate timestamp
msg.header.stamp = self._rospy_time_from_sec(now)
msg.header.frame_id = "0"
npbuf = np.array(buf)
(height, width) = npbuf.shape
msg.height = height
msg.width = width
msg.encoding = "mono8"
msg.step = width
msg.data = npbuf.tostring() # let numpy convert to string
self.pub_image.publish(msg)
self.pub_last_image = now
if self.pub_mean_luminance is not None:
if (now - self.pub_last_mean_luminance) > 0.5:
mean_lum = np.mean(buf)
self.pub_mean_luminance.publish(float(mean_lum))
self.pub_last_mean_luminance = now
buf = FastImage.asfastimage(buf)
ros_list = []
point_list = []
draw_linesegs = [] # [ (x0,y0,x1,y1) ]
if self.enabled.isSet():
light_on_dark = self.light_on_dark.isSet()
offset_x, offset_y = buf_offset
if 1:
# work on a copy of the image so the original is displayed unaltered
copyview = self.copybuf.roi(offset_x, offset_y, buf.size)
buf.get_8u_copy_put(copyview, buf.size)
buf = copyview
buf_view = np.asarray(buf) # get numpy view of data
# step 1. extract points
analysis_radius = self.analysis_radius.get()
luminance_threshold = self.luminance_threshold.get()
if light_on_dark:
clearval = 0
else:
clearval = 255
# outside mask, set values to clearval
mask = self.get_mask()
buf_view[mask] = clearval
for pt_num in range(self.num_points.get()):
if light_on_dark:
# find brightest point
max_val, x, y = buf.max_index(buf.size)
if max_val < luminance_threshold:
break
else:
# find darkest point
min_val, x, y = buf.min_index(buf.size)
if min_val > (255 - luminance_threshold):
break
# calculate region around found point
clearxmin = max(0, x - analysis_radius)
clearxmax = min(buf.size.w - 1, x + analysis_radius)
clearymin = max(0, y - analysis_radius)
clearymax = min(buf.size.h - 1, y + analysis_radius)
# extract a view of this region
this_region = buf_view[clearymin:clearymax, clearxmin:clearxmax]
if light_on_dark:
binary_region = this_region > (max_val / 1.1)
else:
binary_region = this_region < (min_val * 1.1)
num_pixels_classified = np.sum(binary_region.ravel())
# print '%d, (%d, %d)'%(num_pixels_classified,x,y)
if num_pixels_classified > self.max_area.get():
# we don't want this point
pass
else:
# compute luminance center of mass
if not light_on_dark:
# make a light-on-dark image
this_region2 = 255 - this_region
else:
this_region2 = this_region
fibuf = FastImage.asfastimage(this_region2)
try:
results = realtime_image_analysis.py_fit_params(fibuf)
except Exception as err:
print "%s: error extracting image data. ignoring." % (err,)
else:
(x0, y0, area, slope, eccentricity) = results
theta = np.arctan(slope)
x1 = x0 + clearxmin
y1 = y0 + clearymin
# save values
ros_list.append((offset_x + x1, offset_y + y1, theta))
point_list.append((offset_x + x1, offset_y + y1))
# clear the region near the detected point
buf_view[clearymin:clearymax, clearxmin:clearxmax] = clearval
# send data over ROS
if self.num_points.get():
if self.pub_position is not None:
msg = self.pub_position_class()
msg.header.stamp.secs = int(np.floor(timestamp))
msg.header.stamp.nsecs = int((timestamp % 1.0) * 1e9)
msg.header.frame_id = "pixels"
msg.framenumber = framenumber
for (x, y, theta) in ros_list:
pose = self.pub_pose_class()
pose.x = x
pose.y = y
pose.theta = theta
msg.points.append(pose)
self.pub_position.publish(msg)
if self.view_mask_mode.isSet():
w, h = self.image_size
x = self.mask_center_x.get()
y = self.mask_center_y.get()
radius = self.mask_radius.get()
draw_linesegs.extend(lineseg_circle(x, y, radius))
return point_list, draw_linesegs