def read_image(image_msg): try: image = bgr_from_jpg(image_msg.data) return image except ValueError as e: rospy.logerr(e) return None
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 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 = bgr_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') # Set the image to be detected self.detector_used.setImage(image_cv) # Detect lines and normals #rospy.loginfo('segment_max_threshold: %s' % self.segment_max_threshold) white = self.detector_used.detectLines('white', self.segment_max_threshold) yellow = self.detector_used.detectLines('yellow', self.segment_max_threshold) red = self.detector_used.detectLines('red', self.segment_max_threshold) 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 #rospy.loginfo('segmentList: %s' % (len(white.lines)+len(yellow.lines)+len(red.lines))) self.pub_lines.publish(segmentList) tk.completed('--pub_lines--') # VISUALIZATION only below if self.verbose: # print('line_detect_node: verbose is on!') # Draw lines and normals image_with_lines = np.copy(image_cv) 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_used.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 cbNewImage(self, image_msg): # this callback proceeds the latest image, i.e. it applies the current transformation to the image and publishes it # begin2=rospy.Time.now() if not self.thread_lock.acquire(False): return # memorize image self.image_msg = image_msg tk = TimeKeeper(image_msg) try: cv_image = bgr_from_jpg(image_msg.data) except ValueError as e: rospy.loginfo('Anti_instagram cannot decode image: %s' % e) return #cv_image = cv2.imread(cv_image) scale_percent = self.scale_percent width = int(cv_image.shape[1] * scale_percent / 100) height = int(cv_image.shape[0] * scale_percent / 100) dim = (width, height) cv_image = cv2.resize(cv_image, dim, interpolation=cv2.INTER_NEAREST) tk = TimeKeeper(image_msg) # # end0=rospy.Time.now() # duration0=end0-begin2 # rospy.loginfo('Conversion: %s' % duration0) tk.completed('converted') if self.trafo_mode == "cb" or self.trafo_mode == "both": # apply color balance using latest thresholds # begin1=rospy.Time.now() colorBalanced_image_cv2 = self.ai.applyColorBalance( img=cv_image, ThLow=self.ai.ThLow, ThHi=self.ai.ThHi) tk.completed('applyColorBalance') # end1=rospy.Time.now() # duration=end1-begin1 # rospy.loginfo('Complete CB-Trafo Duration: %s' % duration) else: # pass input image colorBalanced_image_cv2 = cv_image if self.trafo_mode == "lin" or self.trafo_mode == "both": # apply color Transform using latest parameters corrected_image_cv2 = self.ai.applyTransform( colorBalanced_image_cv2) tk.completed('applyTransform') else: # pass input image corrected_image_cv2 = colorBalanced_image_cv2 # begin3=rospy.Time.now() # store image to ros message self.corrected_image = self.bridge.cv2_to_compressed_imgmsg( corrected_image_cv2) # , "bgr8") tk.completed('encode') self.corrected_image.header.stamp = image_msg.header.stamp # for synchronization # publish image self.pub_image.publish(self.corrected_image) tk.completed('published') # end3=rospy.Time.now() # duration3=end3-begin3 # rospy.loginfo('Publishing time: %s' % duration3) # begin4=rospy.Time.now() if self.verbose: rospy.loginfo('ai:\n' + tk.getall()) #self.r.sleep() #to keep the rate self.thread_lock.release()
def duckie_detection(self, image_msg): # Decode from compressed image with OpenCV try: image_cv = bgr_from_jpg(image_msg.data) except ValueError as e: print('Could not decode image: %s' % e) return # Switch image from BGR colorspace to HSV hsv = cv2.cvtColor(image_cv, cv2.COLOR_BGR2HSV) # First reduce noise of image by blurring kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (3,3)) orange_mask = cv2.inRange(hsv, (10, 100, 20), (20, 255, 255)) # Helps to remove other blobs erosion_image_cv = cv2.erode(orange_mask,kernel,iterations = 3) # Dilate image that contains what we need after noise removed, (Removes a little noise too) orange_dilate_cv = cv2.dilate(erosion_image_cv, kernel, iterations=1) # Bitwise-AND of mask and yellow only image orange_only_image_cv = cv2.bitwise_and(image_cv, image_cv, mask = orange_dilate_cv) # Detect blobs. reversemask = 255 - orange_dilate_cv keypoints = self.detector.detect(reversemask) # We can keep track of the point with the largest diameter, it is likely a duckie largestBlob = 0 duckPoint = None if len(keypoints) != 0: for points in range(len(keypoints)): x = int(keypoints[points].pt[0]) y = int(keypoints[points].pt[1]) diameter = int(keypoints[0].size) print("Value of X = ", x, "Value of Y = ", y, "Diameter is : ", diameter) if diameter > largestBlob: duckPoint = points largestBlob = diameter duckieX = int(keypoints[duckPoint].pt[0]) duckieY = int(keypoints[duckPoint].pt[1]) duckieD = int(keypoints[duckPoint].size) cv2.putText(image_cv, "Duckie", (duckieX - 20, duckieY - 30), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) # Handle sending UDP message to Server message = str(duckieD) self.clientSocket.sendto(message.encode(), (self.serverName, self.serverPort)) # returnMessage, serverAddress = self.clientSocket.recvfrom(2048) # Draw detected blobs as green circles. # cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures the size of the circle corresponds to the size of blob im_with_keypoints = cv2.drawKeypoints(image_cv, keypoints, np.array([]), (0,255,0), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS) keypoints_image_ros = self.bridge.cv2_to_imgmsg(im_with_keypoints, "bgr8") # Publish images self.publishImage(self.pub_duckie_detection, keypoints_image_ros)
def processImage(self, event): # processes image with either color balance, linear trafo or both # if we have seen an image: if self.image_msg is not None: tk = TimeKeeper(self.image_msg) try: cv_image = bgr_from_jpg(self.image_msg.data) except ValueError as e: rospy.loginfo('Anti_instagram cannot decode image: %s' % e) return tk.completed('converted') # resize input image resized_img = cv2.resize(cv_image, (0, 0), fx=self.resize, fy=self.resize) tk.completed('resized') H, W, D = resized_img.shape # apply geometry if self.fancyGeom: # apply fancy geom mask = self.mask = processGeom2(resized_img) self.mask255 = mask self.mask255[self.mask255 == 1] = 255 self.geomImage = np.expand_dims(mask, axis=-1) * resized_img tk.completed('fancyGeom') # self.geomImage = np.transpose(np.stack((mask, mask, mask), axis=2)) * resized_img # idx = (mask==1) # self.geomImage = np.zeros((H, W), np.uint8) # self.geomImage = resized_img[idx] else: # remove upper part of the image self.geomImage = resized_img[int(H * 0.3):(H - 1), :, :] tk.completed('notFancyGeom') # apply color balance if required if self.trafo_mode == "cb" or self.trafo_mode == "both": # find color balance thresholds start_cb = time.time() self.ai.calculateColorBalanceThreshold(self.geomImage, self.cb_percentage) end_cb = time.time() tk.completed('calculateColorBalanceThresholds') # store color balance thresholds to ros message self.transform_CB.th[0], self.transform_CB.th[ 1], self.transform_CB.th[2] = self.ai.ThLow self.transform_CB.th[3], self.transform_CB.th[ 4], self.transform_CB.th[5] = self.ai.ThHi self.transform_CB.th[3] = 255 self.transform_CB.th[4] = 255 self.transform_CB.th[5] = 255 # publish color balance thresholds self.pub_trafo_CB.publish(self.transform_CB) # rospy.loginfo('ai: Color balance thresholds published.') tk.completed('colorBalance analysis') # apply linear trafo if required if self.trafo_mode == "lin" or self.trafo_mode == "both": # take in account the previous color balance if self.trafo_mode == "both": # apply color balance colorBalanced_image = self.ai.applyColorBalance( self.geomImage, self.ai.ThLow, self.ai.ThHi) rospy.loginfo( 'TRANSFORMATION!!!!!!!!!!!!!!!!!!!!!!!!!!!!!') else: # pass image without color balance trafo colorBalanced_image = self.geomImage tk.completed('passed image to linear trafo') # not yet initialized if not self.initialized: # apply bounded trafo start_lin = time.time() mbool = self.ai.calculateBoundedTransform( colorBalanced_image, self.max_it_1) end_lin = time.time() if mbool: # store color transform to ros message self.transform.s[0], self.transform.s[ 1], self.transform.s[2] = self.ai.shift self.transform.s[3], self.transform.s[ 4], self.transform.s[5] = self.ai.scale # publish color trafo self.pub_trafo.publish(self.transform) rospy.loginfo( 'ai: Initial trafo found and published! Switch to continuous mode.' ) else: rospy.loginfo( 'ai: average error too large. transform NOT updated.' ) # initialisation already done: continuous mode else: # find color transform start_lin2 = time.time() mbool2 = self.ai.calculateBoundedTransform( colorBalanced_image, self.max_it_2) end_lin2 = time.time() if mbool2: tk.completed('calculateTransform') # store color transform to ros message self.transform.s[0], self.transform.s[ 1], self.transform.s[2] = self.ai.shift self.transform.s[3], self.transform.s[ 4], self.transform.s[5] = self.ai.scale # publish color trafo self.pub_trafo.publish(self.transform) else: rospy.loginfo( 'ai: average error too large. transform NOT updated.' ) if self.fancyGeom: self.mask = self.bridge.cv2_to_imgmsg(self.mask255, "mono8") self.pub_mask.publish(self.mask) rospy.loginfo('published mask!') geomImgMsg = self.bridge.cv2_to_imgmsg(self.geomImage, "bgr8") self.pub_geomImage.publish(geomImgMsg) #if self.verbose: # rospy.loginfo('ai:\n' + tk.getall()) if self.trafo_mode == "cb" or self.trafo_mode == "both": cb_time = end_cb - start_cb #print('CB took: ' + str(cb_time)) if self.trafo_mode == "lin" or self.trafo_mode == "both": if not self.initialized: lin1_time = end_lin - start_lin # print('Lin took: ' + str(lin1_time)) else: lin2_time = end_lin2 - start_lin2