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 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)