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 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 show_depth_image( self, image: NDArray[(Any, Any), np.float32], max_depth: int = 1000, points: List[Tuple[int, int]] = [], ) -> None: """ Displays a depth image in grayscale in a window. Args: image: The depth image to display to the screen. max_depth: The farthest depth to show in the image in cm. Anything past this depth is shown as black. points: A list of points in (pixel row, pixel column) format to show on the image as colored dots. Example:: depth_image = rc.camera.get_depth_image() # Show the depth_image captured by the camera. rc.display.show_depth_image(depth_image) # Show anything that is at most 500 cm away, and show a black cross at # row 3, column 5 rc.display.show_depth_image(depth_image, 500, [(3, 5)]) """ if self.__isHeadless: return assert max_depth > 0, "max_depth must be positive." for point in points: assert ( 0 <= point[0] < image.shape[0] and 0 <= point[1] < image.shape[1] ), f"The point [{point}] is not a valid pixel row and column within image." color_image = rc_utils.colormap_depth_image(image, max_depth) # Draw a dot at each point in points for point in points: rc_utils.draw_circle( color_image, point, rc_utils.ColorBGR.green.value, radius=self.__BIG_DOT_RADIUS, ) rc_utils.draw_circle( color_image, point, rc_utils.ColorBGR.blue.value, radius=self.__SMALL_DOT_RADIUS, ) self.show_color_image(color_image)
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 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 show_lidar( self, samples: NDArray[Any, np.float32], radius: int = 128, max_range: int = 1000, highlighted_samples: List[Tuple[float, float]] = [], ) -> None: """ Displays a set of LIDAR samples. Args: samples: A complete LIDAR scan. radius: Half of the width or height (in pixels) of the generated image. max_range: The farthest depth to show in the image in cm. Anything past this depth is shown as black. highlighted_samples: A list of samples in (angle, distance) format to show as light blue dots. Angle must be in degrees from straight ahead (clockwise), and distance must be in cm. Note: Each sample in samples is shown as a red pixel. Each sample in highlighted_samples is shown as a blue pixel. The car is shown as a green dot at the center of the visualization. Warning: samples must be a complete LIDAR scan. This function assumes that each sample is equal angle appart, and that samples spans the entire 360 degrees. If this is not the case, the visualization will be inaccurate. Example:: depth_image = rc.camera.get_depth_image() # Show the depth_image captured by the camera. rc.display.show_depth_image(depth_image) # Show anything that is at most 500 cm away, and show a black cross at # row 3, column 5 rc.display.show_depth_image(depth_image, 500, [(3, 5)]) """ assert radius > 0, "radius must be positive." assert max_range > 0, "max_range must be positive." if self.__isHeadless: return # Create a square black image with the requested radius image = np.zeros((2 * radius, 2 * radius, 3), np.uint8, "C") num_samples: int = len(samples) # Draw a red pixel for each non-zero sample less than max_range for i in range(num_samples): if 0 < samples[i] < max_range: angle: float = 2 * math.pi * i / num_samples length: float = radius * samples[i] / max_range r: int = int(radius - length * math.cos(angle)) c: int = int(radius + length * math.sin(angle)) image[r][c][2] = 255 # Draw a green dot to denote the car rc_utils.draw_circle( image, (radius, radius), rc_utils.ColorBGR.green.value, self.__LIDAR_CAR_RADIUS, ) # Draw a light blue pixel for each point in highlighted_samples for (angle, distance) in highlighted_samples: if 0 < distance < max_range: angle_rad = angle * math.pi / 180 length: float = radius * distance / max_range r: int = int(radius - length * math.cos(angle_rad)) c: int = int(radius + length * math.sin(angle_rad)) image[r][c][0] = 255 image[r][c][1] = 255 image[r][c][2] = 0 self.show_color_image(image)
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 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 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