def ar_in_range(): depth_image = rc.camera.get_depth_image() ar_image = rc.camera.get_color_image() ar_image = rc_utils.crop( ar_image, (0, 0), (rc.camera.get_height() // 2, rc.camera.get_width())) checking_info, checking_info_id = rc_utils.get_ar_markers(ar_image) if checking_info: x = (int)((checking_info[0][0][0][1] + checking_info[0][0][1][1]) // 2) y = (int)((checking_info[0][0][0][0] + checking_info[0][0][1][0]) // 2) if rc_utils.get_pixel_average_distance(depth_image, (x, y)) < 200: contours_ar_orange = rc_utils.find_contours( ar_image, ORANGE[0], ORANGE[1]) contours_ar_purp = rc_utils.find_contours(ar_image, PURPLE[0], PURPLE[1]) orange_largest = rc_utils.get_largest_contour( contours_ar_orange, 2000) purp_largest = rc_utils.get_largest_contour(contours_ar_purp, 2000) if orange_largest is not None: print("orange") return 1 elif purp_largest is not None: print("purple") return 2 else: return 0
def run_slow(self, rc): largest = self.get_2_largest(self.cropped_img, self.slow_col) if self.cur_state == self.State.SLOW: speed = c.SPEED_SLOW else: speed = -1 # If the fast lane is visible more than the slow one, then just keep turning in the previously set direction largest_fast_cropped = rc_utils.get_largest_contour( rc_utils.find_contours(self.cropped_img, self.fast_col.value[0], self.fast_col.value[1])) largest_slow_cropped = rc_utils.get_largest_contour( rc_utils.find_contours(self.cropped_img, self.slow_col.value[0], self.slow_col.value[1])) slow_a = rc_utils.get_contour_area( largest_slow_cropped) if largest_slow_cropped is not None else 0 fast_a = rc_utils.get_contour_area( largest_fast_cropped) if largest_fast_cropped is not None else 0 if len(largest) == 2 and not slow_a < fast_a: self.angle = 0 elif len(largest) == 1 and not slow_a < fast_a: cont = largest[0][1] # Finds the top point of the contour and the bottom (Estimates line slope) top_pt = tuple([ pt[0] for pt in cont if pt[0][1] == np.amin(cont[:, :, 1]) ][0]) bott_pt = tuple([ pt[0] for pt in cont if pt[0][1] == np.amax(cont[:, :, 1]) ][0]) # Slop is sloppy????????????? if self.slow_state_angle == 0: if top_pt[0] - bott_pt[0] > 0: self.slow_state_angle = 1 elif top_pt[0] - bott_pt[0] < 0: self.slow_state_angle = -1 else: self.slow_state_angle = 0 self.angle = self.slow_state_angle # Draws VIZHUALS #cv.line(self.cropped_img, top_pt, bott_pt, (255, 255, 0), thickness=2) #rc.display.show_color_image(self.cropped_img) # Sets speed and angle rc.drive.set_speed_angle(rc_utils.clamp(speed, -1, 1), rc_utils.clamp(self.angle, -1, 1)) return len(largest)
def updateContour(self, rc, depth_image, color_image): if color_image is None: self.contour_center = None else: # Crop the image to the floor directly in front of the car contour_image = rc_utils.crop(color_image, c.LINE_CROP_FLOOR[0], c.LINE_CROP_FLOOR[1]) contours = rc_utils.find_contours(contour_image, self.color.value[0], self.color.value[1]) L_contour = rc_utils.get_largest_contour(contours, c.LINE_MIN_CONTOUR_AREA) if L_contour is not None: self.contour_center = rc_utils.get_contour_center(L_contour) contour_area = rc_utils.get_contour_area(L_contour) # Draw contour onto the image rc_utils.draw_contour(contour_image, L_contour, (0, 255, 255)) rc_utils.draw_circle(contour_image, self.contour_center, (0, 255, 255))
def ar_in_range_color(RANGE, d_img, c_img, colors): #gets depth and ar tags, if there are some, finds center and then compares depth with # RANGE. If within range, prints ids depth_image = d_img ar_image = c_img ar_image = rc_utils.crop( ar_image, (0, 0), (rc.camera.get_height() // 2, rc.camera.get_width())) checking_info, _ = rc_utils.get_ar_markers(ar_image) if checking_info: x = (int)((checking_info[0][0][0][1] + checking_info[0][0][1][1]) // 2) y = (int)((checking_info[0][0][0][0] + checking_info[0][0][1][0]) // 2) if rc_utils.get_pixel_average_distance(depth_image, (x, y)) < RANGE: contours = [ rc_utils.find_contours(ar_image, color.value[0], color.value[1]) for color in colors ] largest_contours = [(idx, rc_utils.get_largest_contour(cont, 2000)) for idx, cont in enumerate(contours)] if len(largest_contours): return colors[max( largest_contours, key=lambda x: get_cont_area_proofed(x[1]))[0]]
def update_contour(): """ Finds contours in the current color image and uses them to update contour_center and contour_area """ global contour_center global contour_area image = rc.camera.get_color_image() if image is None: contour_center = None contour_area = 0 else: # Find all of the orange contours contours = rc_utils.find_contours(image, ORANGE[0], ORANGE[1]) # Select the largest contour contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) if contour is not None: # Calculate contour information contour_center = rc_utils.get_contour_center(contour) contour_area = rc_utils.get_contour_area(contour) # Draw contour onto the image rc_utils.draw_contour(image, contour) rc_utils.draw_circle(image, contour_center) else: contour_center = None contour_area = 0 # Display the image to the screen rc.display.show_color_image(image)
def update_ar(info, info_id, ar_image): global FIRST_PRI1 global SECOND_PRI1 global red_dir global blue_dir global green_dir red_dir = 0 blue_dir = 0 green_dir = 0 #ar_image= rc_utils.crop(image, (0, rc.camera.get_width()//2), (rc.camera.get_height(), rc.camera.get_width())) if info is not None: #checks if there are AR markers, then gets the contour of the outlining color rc_utils.draw_ar_markers(ar_image, info, info_id, (0, 255, 0)) contours_ar_red = rc_utils.find_contours(ar_image, RED[0], RED[1]) contours_ar_blue = rc_utils.find_contours(ar_image, BLUE[0], BLUE[1]) contours_ar_green = rc_utils.find_contours(ar_image, GREEN[0], GREEN[1]) red_largest = rc_utils.get_largest_contour(contours_ar_red, 2000) blue_largest = rc_utils.get_largest_contour(contours_ar_blue, 2000) green_largest = rc_utils.get_largest_contour(contours_ar_green, 2000) if red_largest is not None: redc = rc_utils.get_contour_center(red_largest) red_dir = color_dir(redc, info, red_dir) rc_utils.draw_contour(ar_image, red_largest) if blue_largest is not None: bluec = rc_utils.get_contour_center(blue_largest) blue_dir = color_dir(bluec, info, blue_dir) rc_utils.draw_contour(ar_image, blue_largest) if green_largest is not None: greenc = rc_utils.get_contour_center(green_largest) green_dir = color_dir(greenc, info, green_dir) rc_utils.draw_contour(ar_image, green_largest) # if red_dir != 0 or green_dir != 0 or blue_dir != 0: if red_dir == 0: THIRD_PRI = red_dir elif blue_dir == 0: THIRD_PRI = blue_dir else: THIRD_PRI = green_dir
def get_closest_opposite_contour(self): # Finds closest contour of opposite color opp_col = self.Color.RED if self.__cur_col == self.Color.BLUE else self.Color.BLUE opp_conts = rc_utils.find_contours(self.__color_image, self.COLORS[opp_col.name][0], self.COLORS[opp_col.name][1]) out = P_Slalom.get_closest_contour(opp_conts, self.__depth_image) # Gets the distance to the closest opposite color cone if out is not None: dist_opp = out[0] else: # A really high number that will always be greater than the closest desired cone dist_opp = 99999 return dist_opp
def get_2_largest(self, img, col): # Gets all fast contours and sorts them by contour area contours = list( enumerate(rc_utils.find_contours(img, col.value[0], col.value[1]))) areas = [rc_utils.get_contour_area(cont[1]) for cont in contours] contours = sorted(contours, key=lambda x: areas[x[0]], reverse=True) # Gets 2 largest contours largest = contours[:2] largest = [ cont for cont in largest if areas[cont[0]] > c.LANE_MIN_CONTOUR_AREA ] return largest
def update(): """ After start() is run, this function is run every frame until the back button is pressed """ # TODO: Park the car 30 cm away from the closest orange cone. # Use both color and depth information to handle cones of multiple sizes. # You may wish to copy some of your code from lab2b.py image = rc.camera.get_color_image() contours = rc_utils.find_contours(image, ORANGE[0], ORANGE[1]) contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) if contour is not None: contour_center = rc_utils.get_contour_center(contour) rc_utils.draw_contour(image, contour) rc_utils.draw_circle(image, contour_center) else: contour_center = 0 scan = rc.lidar.get_samples() __, coneDist = rc_utils.get_lidar_closest_point(scan, (-20, 20)) if contour is not None: global pastTerm global derivTerm angleTerm = contour_center[1] - rc.camera.get_width() / 2 speedTerm = coneDist - 50 derivTerm = (speedTerm - pastTerm) / rc.get_delta_time( ) if speedTerm != pastTerm else derivTerm speedSign = speedTerm / abs(speedTerm) if speedTerm != 0 else 0 print(str(speedTerm) + " and " + str(derivTerm)) angle = angleTerm * (1 / 200) #angle P controller speed = speedTerm * (1 / 50) + derivTerm * (1 / 250 ) #speed P"D" controller angle = -1 if angle < -1 else angle angle = 1 if angle > 1 else angle speed = -1 if speed < -1 else speed speed = 1 if speed > 1 else speed #speed = 0 if abs(derivTerm) < 15 else speed else: speed = 0 angle = 0 pastTerm = speedTerm rc.drive.set_speed_angle(speed, angle)
def run_phase(self, rc, depth_image, color_image, lidar_scan): # print("FAST", self.fast_col, "SLOW", self.slow_col) self.cropped_img = np.copy(color_image)[rc.camera.get_height() * 2 // 3:rc.camera.get_height(), :] # SLOW CONTOUR INFO GATHERING # Finds distance to largest slow contour >> largest_slow = rc_utils.get_largest_contour( rc_utils.find_contours(color_image, self.slow_col.value[0], self.slow_col.value[1]), c.LANE_MIN_CONTOUR_AREA) if largest_slow is not None: center_slow = rc_utils.get_contour_center(largest_slow) dist_slow = rc_utils.get_pixel_average_distance( depth_image, center_slow) else: dist_slow = 9999 # -------------------------------------- << if self.cur_state == self.State.FAST: self.run_fast(rc, dist_slow) # If the the slow contour is within a certain range, switch states if dist_slow <= c.STATE_SWITCH_DIST: self.cur_state = self.State.HARD_STOP elif self.cur_state == self.State.SLOW or self.cur_state == self.State.HARD_STOP: if self.cur_state == self.State.HARD_STOP: self.stop_counter += 1 if self.stop_counter >= 10: self.stop_counter = 0 self.cur_state = self.State.SLOW # Runs function and gets output (# of slow contours visible) out = self.run_slow(rc) if out == 0: self.cur_state = self.State.FAST self.slow_state_angle = 0 # If slow line area sum is big enough, align to right side of fast lane: # If no visible fast lane: # ------ Full turn /or/ Consider way to turn on purple line (sharp turn) # If only one line visible: # ------ Save history of left side and right side contours and determine what side the single contour is on """rt = rc.controller.get_trigger(rc.controller.Trigger.RIGHT)
def update_contour(): """ Finds contours in the current color image and uses them to update contour_center and contour_area """ global contour_center global contour_area image = rc.camera.get_color_image() if image is None: contour_center = None contour_area = 0 else: # TODO (challenge 1): Search for multiple tape colors with a priority order DONE # (currently we only search for blue) # Crop the image to the floor directly in front of the car image = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1]) # Find all of the colored contours for color in color_priority: contours = rc_utils.find_contours(image, color[0], color[1]) if len(contours) > 0: break # Select the largest contour contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) if contour is not None: # Calculate contour information contour_center = rc_utils.get_contour_center(contour) contour_area = rc_utils.get_contour_area(contour) # Draw contour onto the image rc_utils.draw_contour(image, contour) rc_utils.draw_circle(image, contour_center) else: contour_center = None contour_area = 0 # Display the image to the screen rc.display.show_color_image(image)
def update_contour(): """ Finds contours in the current color image and uses them to update contour_center and contour_area """ global contour_center global contour_area image = rc.camera.get_color_image() if image is None: contour_center = None contour_area = 0 else: # Crop the image to the floor directly in front of the car image = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1]) # Search for each color in priority order for color in COLOR_PRIORITY: # Find all of the contours of the current color contours = rc_utils.find_contours(image, color[0], color[1]) # Select the largest contour contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) if contour is not None: # Calculate contour information contour_center = rc_utils.get_contour_center(contour) contour_area = rc_utils.get_contour_area(contour) # Draw contour onto the image rc_utils.draw_contour(image, contour) rc_utils.draw_circle(image, contour_center) break # If no contours are found for any color, set center and area accordingly else: contour_center = None contour_area = 0 # Display the image to the screen rc.display.show_color_image(image)
def update_contours(self, rc): contours = rc_utils.find_contours(self.__color_image, self.COLORS[self.__cur_col.name][0], self.COLORS[self.__cur_col.name][1]) #get contours for color, find closest one out = P_Slalom.get_closest_contour(contours, self.__depth_image) if out is not None: # Saves previous measured distance #if self.__contour_distance is not None: #self.__prev_distance = self.__contour_distance self.__prev_distance = self.__contour_distance self.__contour_distance, self.__contour, self.__contour_center = out # Draws closest contour rc_utils.draw_contour(self.__color_image, self.__contour) rc_utils.draw_circle(self.__color_image, self.__contour_center) else: self.__contour_distance = None self.__contour_center = None self.__contour = None
def updateContour(color_priority): image = rc.camera.get_color_image() if image is None: contour_center = None contour_area = 0 else: image_cropped = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1]) for color in color_priority: contours = rc_utils.find_contours(image_cropped,color[0],color[1]) if len(contours) > 0: break contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) if contour is not None: contour_center = rc_utils.get_contour_center(contour) contour_area = rc_utils.get_contour_area(contour) else: contour_center = None contour_area = 0 rc.display.show_color_image(image) return contour_center,contour_area
def check(image, co): contours = rc_utils.find_contours(image, co[0], co[1]) contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) return contour
def update(): """ After start() is run, this function is run every frame until the back button is pressed """ # Display the color image cropped to the top left if rc.controller.was_pressed(rc.controller.Button.A): image = rc.camera.get_color_image() cropped = rc_utils.crop( image, (0, 0), (rc.camera.get_height() // 2, rc.camera.get_width() // 2)) rc.display.show_color_image(cropped) # Find and display the largest red contour in the color image if rc.controller.was_pressed(rc.controller.Button.B): image = rc.camera.get_color_image() contours = rc_utils.find_contours(image, RED[0], RED[1]) largest_contour = rc_utils.get_largest_contour(contours) if largest_contour is not None: center = rc_utils.get_contour_center(largest_contour) area = rc_utils.get_contour_area(largest_contour) print("Largest red contour: center={}, area={:.2f}".format( center, area)) rc_utils.draw_contour(image, largest_contour, rc_utils.ColorBGR.green.value) rc_utils.draw_circle(image, center, rc_utils.ColorBGR.yellow.value) rc.display.show_color_image(image) else: print("No red contours found") # Print depth image statistics and show the cropped upper half if rc.controller.was_pressed(rc.controller.Button.X): depth_image = rc.camera.get_depth_image() # Measure average distance at several points left_distance = rc_utils.get_pixel_average_distance( depth_image, (rc.camera.get_height() // 2, rc.camera.get_width() // 4), ) center_distance = rc_utils.get_depth_image_center_distance(depth_image) center_distance_raw = rc_utils.get_depth_image_center_distance( depth_image, 1) right_distance = rc_utils.get_pixel_average_distance( depth_image, (rc.camera.get_height() // 2, 3 * rc.camera.get_width() // 4), ) print(f"Depth image left distance: {left_distance:.2f} cm") print(f"Depth image center distance: {center_distance:.2f} cm") print(f"Depth image raw center distance: {center_distance_raw:.2f} cm") print(f"Depth image right distance: {right_distance:.2f} cm") # Measure pixels where the kernel falls off the edge of the photo upper_left_distance = rc_utils.get_pixel_average_distance( depth_image, (2, 1), 11) lower_right_distance = rc_utils.get_pixel_average_distance( depth_image, (rc.camera.get_height() - 2, rc.camera.get_width() - 5), 13) print(f"Depth image upper left distance: {upper_left_distance:.2f} cm") print( f"Depth image lower right distance: {lower_right_distance:.2f} cm") # Find closest point in bottom third cropped = rc_utils.crop( depth_image, (0, 0), (rc.camera.get_height() * 2 // 3, rc.camera.get_width()), ) closest_point = rc_utils.get_closest_pixel(cropped) closest_distance = cropped[closest_point[0]][closest_point[1]] print( f"Depth image closest point (upper half): (row={closest_point[0]}, col={closest_point[1]}), distance={closest_distance:.2f} cm" ) rc.display.show_depth_image(cropped, points=[closest_point]) # Print lidar statistics and show visualization with closest point highlighted if rc.controller.was_pressed(rc.controller.Button.Y): lidar = rc.lidar.get_samples() front_distance = rc_utils.get_lidar_average_distance(lidar, 0) right_distance = rc_utils.get_lidar_average_distance(lidar, 90) back_distance = rc_utils.get_lidar_average_distance(lidar, 180) left_distance = rc_utils.get_lidar_average_distance(lidar, 270) print(f"Front LIDAR distance: {front_distance:.2f} cm") print(f"Right LIDAR distance: {right_distance:.2f} cm") print(f"Back LIDAR distance: {back_distance:.2f} cm") print(f"Left LIDAR distance: {left_distance:.2f} cm") closest_sample = rc_utils.get_lidar_closest_point(lidar) print( f"Closest LIDAR point: {closest_sample[0]:.2f} degrees, {closest_sample[1]:.2f} cm" ) rc.display.show_lidar(lidar, highlighted_samples=[closest_sample]) # Print lidar distance in the direction the right joystick is pointed rjoy_x, rjoy_y = rc.controller.get_joystick(rc.controller.Joystick.RIGHT) if abs(rjoy_x) > 0 or abs(rjoy_y) > 0: lidar = rc.lidar.get_samples() angle = (math.atan2(rjoy_x, rjoy_y) * 180 / math.pi) % 360 distance = rc_utils.get_lidar_average_distance(lidar, angle) print(f"LIDAR distance at angle {angle:.2f} = {distance:.2f} cm") # Default drive-style controls left_trigger = rc.controller.get_trigger(rc.controller.Trigger.LEFT) right_trigger = rc.controller.get_trigger(rc.controller.Trigger.RIGHT) left_joystick = rc.controller.get_joystick(rc.controller.Joystick.LEFT) rc.drive.set_speed_angle(right_trigger - left_trigger, left_joystick[0])
def find_cones(): """ Find the closest red and blue cones and update corresponding global variables. """ global red_center global red_distance global prev_red_distance global blue_center global blue_distance global prev_blue_distance prev_red_distance = red_distance prev_blue_distance = blue_distance color_image = rc.camera.get_color_image() depth_image = rc.camera.get_depth_image() if color_image is None or depth_image is None: red_center = None red_distance = 0 blue_center = None blue_distance = 0 print("No image found") return # Search for the red cone contours = rc_utils.find_contours(color_image, RED[0], RED[1]) contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) if contour is not None: red_center = rc_utils.get_contour_center(contour) red_distance = rc_utils.get_pixel_average_distance(depth_image, red_center) # Only use count it if the cone is less than MAX_DISTANCE away if red_distance <= MAX_DISTANCE: rc_utils.draw_contour(color_image, contour, rc_utils.ColorBGR.green.value) rc_utils.draw_circle(color_image, red_center, rc_utils.ColorBGR.green.value) else: red_center = None red_distance = 0 else: red_center = None red_distance = 0 # Search for the blue cone contours = rc_utils.find_contours(color_image, BLUE[0], BLUE[1]) contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) if contour is not None: blue_center = rc_utils.get_contour_center(contour) blue_distance = rc_utils.get_pixel_average_distance(depth_image, blue_center) # Only use count it if the cone is less than MAX_DISTANCE away if blue_distance <= MAX_DISTANCE: rc_utils.draw_contour(color_image, contour, rc_utils.ColorBGR.yellow.value) rc_utils.draw_circle( color_image, blue_center, rc_utils.ColorBGR.yellow.value ) else: blue_center = None blue_distance = 0 else: blue_center = None blue_distance = 0 rc.display.show_color_image(color_image)
def checkYellow(image): global YELLOW contours = rc_utils.find_contours(image, YELLOW_TAPE[0], YELLOW_TAPE[1]) contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) return contour
def checkOrange(image): global ORANGE contours = rc_utils.find_contours(image, NEON_ORANGE_CONE[0], NEON_ORANGE_CONE[1]) contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) return contour
def checkGreen(image): global GREEN contours = rc_utils.find_contours(image, NEON_GREEN_CONE[0], NEON_GREEN_CONE[1]) contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) return contour
def checkRed(image): global RED contours = rc_utils.find_contours(image, RED[0], RED[1]) contour = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) return contour
def flane(color_lane): global contour_center global contour_area global speed global angle global lane global arcount global a global time time += rc.get_delta_time() cimage = rc.camera.get_color_image() if cimage is None: contour_center = None contour_area = 0 else: #splits image in two so that two separate contours followed left = rc_utils.crop( cimage, (360, 0), (rc.camera.get_height(), rc.camera.get_width() // 2)) right = rc_utils.crop(cimage, (360, rc.camera.get_width() // 2), (rc.camera.get_height(), rc.camera.get_width())) both = [left, right] contour_centers = [] contour_areas = [] #find largest contours of left contours_pur = rc_utils.find_contours(left, color_lane[0], color_lane[1]) print(contours_pur) rc.display.show_color_image(left) print(color_lane) contour_left = rc_utils.get_largest_contour(contours_pur, MIN_CONTOUR_AREA) #if there are contours, finds center and adds to contour_centers list if contour_left is not None: left_center = rc_utils.get_contour_center(contour_left) contour_centers.append(left_center) rc_utils.draw_contour(left, contour_left, (255, 0, 0)) rc.display.show_color_image(left) else: contour_centers.append(None) #find largest contours of right contours = rc_utils.find_contours(right, color_lane[0], color_lane[1]) contour_right = rc_utils.get_largest_contour(contours, MIN_CONTOUR_AREA) #if there are contours, finds center and adds to contour_centers list if contour_right is not None: right_center = rc_utils.get_contour_center(contour_right) contour_centers.append(right_center) #rc_utils.draw_contour(right, contour_right) #rc.display.show_color_image(right) else: contour_centers.append(None) #adjusts car based on being in the center of two contours if None not in contour_centers: lane = True image = rc.camera.get_color_image() contour_distance = ( contour_centers[1][1] + rc.camera.get_width() // 2) - contour_centers[0][1] contour_center = (contour_centers[0][0] + 10, (contour_distance // 2) + contour_centers[0][1]) else: contour_center = None speed = 1 angle = 0 if lane == True: #catches if goes off lane, else, adjusts based off of center of contours if contour_centers[0] == None: print("turn left") angle = -1 elif contour_centers[1] == None: print("turn right") angle = 1 else: angle = rc_utils.remap_range(contour_center[1], 0, rc.camera.get_width(), -1, 1, True) speed = 1 ##getting ar direction if arcount == 0 and time > 2: a = rc.camera.get_color_image() a = rc_utils.crop( a, (0, rc.camera.get_width() // 4), (rc.camera.get_height(), rc.camera.get_width() - (rc.camera.get_width() // 4))) corners, ids = rc_utils.get_ar_markers(a) rc_utils.draw_ar_markers(a, corners, ids) if len(corners) > 0: #print("getting dir") if rc_utils.get_ar_direction(corners[0]) == Direction.LEFT: print(rc_utils.get_ar_direction(corners[0])) angle = -0.7 if rc_utils.get_ar_direction(corners[0]): print(rc_utils.get_ar_direction(corners[0])) angle = 0.7 speed = 1 rc.drive.set_speed_angle(0, 0)
def update_contour(): """ Finds contours in the current color image and uses them to update contour_center and contour_area """ global contour_center global contour_area global cur_state global FIRST_PRI1 global SECOND_PRI1 #global THIRD_PRI global red_dir global blue_dir global green_dir contour_image = rc.camera.get_color_image() if contour_image is None: contour_center = None contour_area = 0 else: # TODO (challenge 1): Search for multiple tape colors with a priority order # (currently we only search for blue) # Crop the image to the floor directly in front of the car contour_image = rc_utils.crop(contour_image, CROP_FLOOR[0], CROP_FLOOR[1]) #Find all of the red contours contours_red = rc_utils.find_contours(contour_image, RED[0], RED[1]) # Find all of the blue contours contours_blue = rc_utils.find_contours(contour_image, BLUE[0], BLUE[1]) #Find all of the green contours contours_green = rc_utils.find_contours(contour_image, GREEN[0], GREEN[1]) # Select the largest contour L_contour_blue = rc_utils.get_largest_contour(contours_blue, MIN_CONTOUR_AREA) L_contour_red = rc_utils.get_largest_contour(contours_red, MIN_CONTOUR_AREA) L_contour_green = rc_utils.get_largest_contour(contours_green, MIN_CONTOUR_AREA) # Priorities##################################################################### if FIRST_PRI1: if FIRST_PRI1 == red_dir: FIRST_PRI = L_contour_red if SECOND_PRI1 == blue_dir: SECOND_PRI = L_contour_blue THIRD_PRI = L_contour_green else: SECOND_PRI = L_contour_green THIRD_PRI = L_contour_blue elif FIRST_PRI1 == blue_dir: FIRST_PRI = L_contour_blue if SECOND_PRI1 == green_dir: SECOND_PRI = L_contour_green THIRD_PRI = L_contour_red else: SECOND_PRI = L_contour_red THIRD_PRI = L_contour_green elif FIRST_PRI1 == green_dir: FIRST_PRI = L_contour_green if SECOND_PRI1 == blue_dir: SECOND_PRI = L_contour_blue THIRD_PRI = L_contour_red else: SECOND_PRI = L_contour_red THIRD_PRI = L_contour_blue if FIRST_PRI is not None: # and contour_center_first<200: # Calculate contour information contour_center = rc_utils.get_contour_center(FIRST_PRI) contour_area = rc_utils.get_contour_area(FIRST_PRI) # Draw contour onto the image rc_utils.draw_contour(contour_image, FIRST_PRI, (0, 255, 0)) rc_utils.draw_circle(contour_image, contour_center) elif SECOND_PRI is not None: # Calculate contour information contour_center = rc_utils.get_contour_center(SECOND_PRI) contour_area = rc_utils.get_contour_area(SECOND_PRI) # Draw contour onto the image rc_utils.draw_contour(contour_image, SECOND_PRI, (0, 0, 255)) rc_utils.draw_circle(contour_image, contour_center) elif THIRD_PRI is not None: # Calculate contour information contour_center = rc_utils.get_contour_center(THIRD_PRI) contour_area = rc_utils.get_contour_area(THIRD_PRI) # Draw contour onto the image rc_utils.draw_contour(contour_image, THIRD_PRI, (255, 0, 0)) rc_utils.draw_circle(contour_image, contour_center) else: contour_center = None contour_area = 0