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 update(): """ After start() is run, this function is run every frame until the back button is pressed """ color_image = rc.camera.get_color_image() markers = rc_utils.get_ar_markers(color_image)
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 ar_in_range_ID(RANGE, d_img, c_img): #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, 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)) < RANGE: return (checking_info_id) return None
def start(): """ This function is run once every time the start button is pressed """ global cur_state global ar_image # Have the car begin at a stop rc.drive.stop() 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())) info, info_id = rc_utils.get_ar_markers(ar_image) update_ar(info, info_id, ar_image) cur_state = State.laneFollow #cur_state = State.lineFollow # Print start message print(">> Final Challenge - Time Trials")
def update(): """ After start() is run, this function is run every frame until the back button is pressed """ global speed global angle global cur_state global PRIORITY global prevangle global cones_done global cur_mode global counter # Get all images image = rc.camera.get_color_image() #cur_state == State.cone_slaloming corners, ids = rc_utils.get_ar_markers(image) length = len(corners) if length > 0: id = 300 index = 0 for idx in range(0, len(ids)): if ids[idx] < id: id = ids[idx] index = idx TL = corners[index][0][0] TR = corners[index][0][1] BL = corners[index][0][3] area = (abs(TL[0] - TR[0]) + abs(TL[1] - TR[1])) * (abs(TL[0] - BL[0]) + abs(TL[1] - BL[1])) print(id[0], area) if id[0] == 32 and area > 1900: if cur_state is not State.cone_slaloming: cur_mode = Mode.no_cones counter = 0 cur_state = State.cone_slaloming print("State: ", cur_state) elif id[0] == 236 and area > 850: cur_state = State.wall_parking print("State: ", cur_state) depth_image = rc.camera.get_depth_image() ###### Line Following State ###### if cur_state == State.line_following: if image is None: contour_center = None else: # Crop the image to the floor directly in front of the car image = rc_utils.crop(image, CROP_FLOOR[0], CROP_FLOOR[1]) colorContours = [] contour = None colorContours = [] red = checkRed(image) green = checkGreen(image) #blue = checkBlue(image) yellow = checkYellow(image) for priority in PRIORITY: if priority == "Y" and yellow is not None: colorContours.append(yellow) print("yellow") elif priority == "R" and red is not None: colorContours.append(red) print("red") elif priority == "G" and green is not None: colorContours.append(green) print("green") if not colorContours: angle = prevangle contour = None else: contour = colorContours[0] if contour is not None: # Calculate contour information contour_center = rc_utils.get_contour_center(contour) # Draw contour onto the image rc_utils.draw_contour(image, contour) rc_utils.draw_circle(image, contour_center) #change else: contour_center = None if contour_center is not None: angle = rc_utils.remap_range(contour_center[1], 0, rc.camera.get_width(), -1, 1, True) angle = rc_utils.clamp(angle, -1, 1) prevangle = angle # Display the image to the screen rc.display.show_color_image(image) ##### Cone Slaloming State ###### elif cur_state == State.cone_slaloming: print("cone slaloming") update_cones() ###### Wall Parking State ###### elif cur_state == State.wall_parking: print("Wall Parking") # Get distance at 1/4, 2/4, and 3/4 width center_dist = rc_utils.get_depth_image_center_distance(depth_image) left_dist = rc_utils.get_pixel_average_distance( depth_image, LEFT_POINT, KERNEL_SIZE) right_dist = rc_utils.get_pixel_average_distance( depth_image, RIGHT_POINT, KERNEL_SIZE) print("distance", center_dist) # Get difference between left and right distances dist_dif = left_dist - right_dist print("dist_dif", dist_dif) # Remap angle angle = rc_utils.remap_range(dist_dif, -MAX_DIST_DIF, MAX_DIST_DIF, -1, 1, True) if abs(dist_dif) > 1: print("entered") angle = rc_utils.remap_range(dist_dif, -MAX_DIST_DIF, MAX_DIST_DIF, -1, 1, True) if center_dist > 20: speed = 0.5 elif center_dist < 21 and center_dist > 10: speed = rc_utils.remap_range(center_dist, 20, 10, 0.5, 0) speed = rc_utils.clamp(speed, 0, 0.5) else: speed = 0 print("speed", speed) rc.drive.set_speed_angle(speed, angle) else: # stop moving rc.drive.stop() print("angle", angle) print("speed", speed) rc.drive.set_speed_angle(0.6, angle)
def run_phase(self, rc, depth_image, color_image, lidar_scan): #print(">> Running Wall Following") """ After start() is run, this function is run every frame until the back button is pressed """ # Use the triggers to control the car's speed left joystick for angle #rt = rc.controller.get_trigger(rc.controller.Trigger.RIGHT) #lt = rc.controller.get_trigger(rc.controller.Trigger.LEFT) #speed = rt - lt #angle = rc.controller.get_joystick(rc.controller.Joystick.LEFT)[0] # Calculate the distance scan = lidar_scan max_wall = 0.65 #testing max speed hall = rc.camera.get_width() // 9 optimum = 60 rightDist = rc_utils.get_lidar_average_distance(scan, 44, 10) leftDist = rc_utils.get_lidar_average_distance(scan, 316, 10) angle = rc_utils.remap_range(rightDist - leftDist, -hall, hall, -1, 1) angle = rc_utils.clamp(angle, -1, 1) # get them tags corners, ids = rc_utils.get_ar_markers(color_image) billy = 150 if c.ar_in_range_ID(billy, depth_image, color_image, rc, 4 / 5) == c.ID_DIRECTION: dirrrrrrrrr = rc_utils.get_ar_direction(corners[0]) #print(dirrrrrrrrr) if dirrrrrrrrr == rc_utils.Direction.LEFT: angle = -1 elif dirrrrrrrrr == rc_utils.Direction.RIGHT: angle = 1 self.ar_tag = True elif self.is_canyon: tooClose = 80 if rc_utils.get_depth_image_center_distance( depth_image) < tooClose: angle = 1 right_farthest = np.amax( depth_image[rc.camera.get_height() * 5 // 6, rc.camera.get_width() // 2:rc.camera.get_width()].flatten()) left_farthest = np.amax( depth_image[rc.camera.get_height() * 5 // 6, 0:rc.camera.get_width() // 2].flatten()) diff = abs(right_farthest - left_farthest) AAAAAAAAAH_WE_ARE_ABOUT_TO_FALL____BETTER_STOP_NOW = 100 if self.ar_tag and self.ledge_count == 0 and diff > 50: if right_farthest > AAAAAAAAAH_WE_ARE_ABOUT_TO_FALL____BETTER_STOP_NOW: self.many += 1 self.ledge_angle = -1 self.ledge_count = 10 elif left_farthest > AAAAAAAAAH_WE_ARE_ABOUT_TO_FALL____BETTER_STOP_NOW: self.many += 1 self.ledge_angle = 1 self.ledge_count = 10 #print("left ", left_farthest, " right ", right_farthest) speed = rc_utils.remap_range(abs(angle), 15, 1, 1, 0.5) #temp controls if self.many == 3: self.ar_tag = False if self.ledge_count > 0: angle = self.ledge_angle self.ledge_count -= 1 rc.drive.set_speed_angle(max_wall * speed, angle)
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(): """ 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(f"Largest red contour: center={center}, area={area:.2f}") 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]) # Identify AR markers if rc.controller.was_pressed(rc.controller.Button.RB): image = rc.camera.get_color_image() markers = rc_utils.get_ar_markers(image, COLORS) for i in range(len(markers)): print(f"AR Marker {i}:") print(markers[i]) print("") rc_utils.draw_ar_markers(image, markers) rc.display.show_color_image(image) if rc.controller.was_pressed(rc.controller.Button.LB): raise Exception("The left bumper was pressed") # Print lidar distance in the direction the right joystick is pointed, if clicked if rc.controller.is_down(rc.controller.Button.RJOY): rjoy_x, rjoy_y = rc.controller.get_joystick( rc.controller.Joystick.RIGHT) 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])