def _draw_points(self, image, points):
"""Take an Image message and a Points message, draw a boc around the
points in the image and publish the result.
Record *image*'s sequence number in _last_published_seq.
"""
# Convert image into RGB image
rgb_im = image_to_array(image)
# Use OpenCV to draw boxes in image
for point in points.points:
roi = point.roi
# NB: cv2.circle will modify the image it works on.
if (roi.x_offset, roi.y_offset)!=(0,0):
cv2.circle(rgb_im,
(roi.x_offset, roi.y_offset),
5,
(255, 0, 0), # green
2 # thickness
)
# Publish image
self._image_pub.publish(array_to_image(rgb_im))
self._last_published_seq = image.header.seq
def _draw_points(self, image, points):
"""Take an Image message and a Points message, draw a boc around the
points in the image and publish the result.
Record *image*'s sequence number in _last_published_seq.
"""
# Convert image into RGB image
rgb_im = image_to_array(image)
# Use OpenCV to draw boxes in image
for point in points.points:
roi = point.roi
# NB: cv2.circle will modify the image it works on.
if (roi.x_offset, roi.y_offset) != (0, 0):
cv2.circle(
rgb_im,
(roi.x_offset, roi.y_offset),
5,
(255, 0, 0), # green
2 # thickness
)
# Publish image
self._image_pub.publish(array_to_image(rgb_im))
self._last_published_seq = image.header.seq
def _detect_points_callback(self, event):
"""Called periodically to detect points in an image and publish the
result.
"""
# Get the latest image to arrive
image = self._latest_image
# Don't do anything if there is no latest image
if image is None:
return
# "Claim" this image by clearing _latest_image
self._latest_image = None
# Create the point message we will eventually publish
points_msg = Points()
# Copy the header from the input image so we know when and what these
# point detection results relate to.
points_msg.header = image.header
# Parse image message into numpy array
image_array = image_to_array(image)
rospy.logdebug('Successfully parsed new image with shape: %s',
image_array.shape)
imageArray = reduce_size(image_array,'rgb',2);
#imageArray = reduce_size(imageArray,'rgb',2);
#imageArray = reduce_size(imageArray,'rgb',2);
# Detect points
detect_point = PointDetector
points = detect_point(image_array)
# Create a point bounding circle for each point
for point_idx, point_circle in enumerate(points):
rospy.logdebug('Point #%s at %s', point_idx+1, point_circle)
# Create point message
point_msg = Point()
point_msg.header = points_msg.header
# Initialise roi
if points != (0,0):
point_msg.roi.x_offset = point_circle[0]
point_msg.roi.y_offset = point_circle[1]
point_msg.roi.do_rectify = False
# Append to list of points
points_msg.points.append(point_msg)
# Publish points messages
self._points_pub.publish(points_msg)
rospy.logdebug('Coordinates of point: %s',
points)
def _detect_points_callback(self, event):
"""Called periodically to detect points in an image and publish the
result.
"""
# Get the latest image to arrive
image = self._latest_image
# Don't do anything if there is no latest image
if image is None:
return
# "Claim" this image by clearing _latest_image
self._latest_image = None
# Create the point message we will eventually publish
points_msg = Points()
# Copy the header from the input image so we know when and what these
# point detection results relate to.
points_msg.header = image.header
# Parse image message into numpy array
image_array = image_to_array(image)
rospy.logdebug('Successfully parsed new image with shape: %s',
image_array.shape)
imageArray = reduce_size(image_array, 'rgb', 2)
#imageArray = reduce_size(imageArray,'rgb',2);
#imageArray = reduce_size(imageArray,'rgb',2);
# Detect points
detect_point = PointDetector
points = detect_point(image_array)
# Create a point bounding circle for each point
for point_idx, point_circle in enumerate(points):
rospy.logdebug('Point #%s at %s', point_idx + 1, point_circle)
# Create point message
point_msg = Point()
point_msg.header = points_msg.header
# Initialise roi
if points != (0, 0):
point_msg.roi.x_offset = point_circle[0]
point_msg.roi.y_offset = point_circle[1]
point_msg.roi.do_rectify = False
# Append to list of points
points_msg.points.append(point_msg)
# Publish points messages
self._points_pub.publish(points_msg)
rospy.logdebug('Coordinates of point: %s', points)
