def handle_images():
tip_msg = Tip()
# Set "header" for tip msg
tip_msg.header = STATE["last_image_header"]
# TODO: process STATE["left"] and STATE["right"] and get actual tip location
# Detect tips in the image
detect_tip = TipDetector
left_image = STATE["left"]
#tips2d = detect_tip(left_image)
# fake tips2d
tips2d=(351,328)
# Convert to robot's world coordinates
convert_world = world_coordinates
tips3d = convert_world(tips2d[0],tips2d[1],STATE["left"],STATE["right"])
# Set positions to real values
tip_msg.x = tips3d[0] #np.random.random()
tip_msg.y = tips3d[1] #np.random.random()
tip_msg.z = tips3d[2] #np.random.random()
# Publish the tip message to the other nodes which are interested
STATE["tip_publisher"].publish(tip_msg)
def _detect_tips_callback(self, event):
"""Called periodically to detect tips 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 tip message we will eventually publish
tips_msg = Tips()
# Copy the header from the input image so we know when and what these
# tip detection results relate to.
tips_msg.header = image.header
# Parse image message into numpy array
image_array = image_to_array(image)
imageArray = reduce_size(image_array,'rgb',2);
#imageArray = reduce_size(imageArray,'rgb',2);
#imageArray = reduce_size(imageArray,'rgb',2);
# Detect tips
#detect_tip = TipDetector
#tips = detect_tip(imageArray)
tips = (330, 100)
# Create a tip bounding box for each tip
#for tip_idx, tip_bbox in enumerate(tips):
# rospy.logdebug('Tip #%s at %s', tip_idx+1, tip_bbox)
# Create tip message
tip_msg = Tip()
tip_msg.header = tips_msg.header
# Initialise roi
if tips != (0,0):
tip_msg.roi.x_offset = tips[0]
tip_msg.roi.y_offset = tips[1]
tip_msg.roi.do_rectify = False
# Append to list of tips
tips_msg.tips.append(tip_msg)
# Publish tips messages
self._tips_pub.publish(tips_msg)
rospy.logdebug('Coordinates of tip: %s',
tips)
def _detect_tips_callback(self, event):
"""Called periodically to detect tips 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 tip message we will eventually publish
tips_msg = Tips()
# Copy the header from the input image so we know when and what these
# tip detection results relate to.
tips_msg.header = image.header
# Parse image message into numpy array
image_array = image_to_array(image)
imageArray = reduce_size(image_array, 'rgb', 2)
#imageArray = reduce_size(imageArray,'rgb',2);
#imageArray = reduce_size(imageArray,'rgb',2);
# Detect tips
#detect_tip = TipDetector
#tips = detect_tip(imageArray)
tips = (330, 100)
# Create a tip bounding box for each tip
#for tip_idx, tip_bbox in enumerate(tips):
# rospy.logdebug('Tip #%s at %s', tip_idx+1, tip_bbox)
# Create tip message
tip_msg = Tip()
tip_msg.header = tips_msg.header
# Initialise roi
if tips != (0, 0):
tip_msg.roi.x_offset = tips[0]
tip_msg.roi.y_offset = tips[1]
tip_msg.roi.do_rectify = False
# Append to list of tips
tips_msg.tips.append(tip_msg)
# Publish tips messages
self._tips_pub.publish(tips_msg)
rospy.logdebug('Coordinates of tip: %s', tips)
def handle_images():
tip_msg = Tip()
# Set "header" for tip msg
tip_msg.header = STATE["last_image_header"]
# TODO: process STATE["left"] and STATE["right"] and get actual tip location
# Detect tips in the image
detect_tip = TipDetector
left_image = STATE["left"]
#tips2d = detect_tip(left_image)
# fake tips2d
tips2d = (351, 328)
# Convert to robot's world coordinates
convert_world = world_coordinates
tips3d = convert_world(tips2d[0], tips2d[1], STATE["left"], STATE["right"])
# Set positions to real values
tip_msg.x = tips3d[0] #np.random.random()
tip_msg.y = tips3d[1] #np.random.random()
tip_msg.z = tips3d[2] #np.random.random()
# Publish the tip message to the other nodes which are interested
STATE["tip_publisher"].publish(tip_msg)
